private void set(CircleVertices other) { int numberOfVertices = getNumberOfVertices(); for (int i = 0; i < numberOfVertices; i++) { vertices[i].set(other.vertices[i]); normals[i].set(other.normals[i]); } }
public CircleVertices(int numberOfVertices) { vertices = new Point3D32[numberOfVertices]; normals = new Vector3D32[numberOfVertices]; for (int i = 0; i < numberOfVertices; i++) { vertices[i] = new Point3D32(); normals[i] = new Vector3D32(); } for (int i = 0; i < numberOfVertices; i++) { double angle = i / (double) numberOfVertices * 2.0 * Math.PI; double ux = Math.cos(angle); double uy = Math.sin(angle); vertices[i].set(radius * ux, radius * uy, 0.0); normals[i].set(ux, uy, 0.0); } }
private static Vector3D32[] findNormalsPerFace(int[] indices, Point3D32[] vertices) { Vector3D32[] normalsPerFace = new Vector3D32[indices.length / 3]; // Abuse integer division. Vector3D32 firstVector = new Vector3D32(); Vector3D32 secondVector = new Vector3D32(); Point3D32[] faceVertices = new Point3D32[3]; for (int face = 0; face < normalsPerFace.length; face++) { normalsPerFace[face] = new Vector3D32(); for (int i = 0; i < faceVertices.length; i++) { faceVertices[i] = vertices[indices[face * 3 + i]]; } firstVector.set(faceVertices[2]); firstVector.sub(faceVertices[0]); secondVector.set(faceVertices[2]); secondVector.sub(faceVertices[1]); normalsPerFace[face].cross(firstVector, secondVector); normalsPerFace[face].normalize(); } return normalsPerFace; }
texturePoints.add().set(texturePoint); for (Vector3D32 normal : meshDataHolder.getVertexNormals()) vertexNormals.add().set(normal);
/** * Append a mesh to this. * @param other the mesh to append. Not modified. * @param updateTriangleIndices whether the indices of the given mesh should be updated when appended. Highly recommended, set it to false only if you what you are doing. */ public void add(ModifiableMeshDataHolder other, boolean updateTriangleIndices) { if (updateTriangleIndices) { int shift = vertices.size(); for (int i = 0; i < other.triangleIndices.size(); i++) triangleIndices.add(other.triangleIndices.get(i) + shift); } else { triangleIndices.addAll(other.triangleIndices); } for (int i = 0; i < other.vertices.size(); i++) vertices.add().set(other.vertices.get(i)); for (int i = 0; i < other.texturePoints.size(); i++) texturePoints.add().set(other.texturePoints.get(i)); for (int i = 0; i < other.vertexNormals.size(); i++) vertexNormals.add().set(other.vertexNormals.get(i)); } }
public void rotate(Matrix3DReadOnly rotationMatrix) { for (Point3D32 vertex : vertices) { double x = rotationMatrix.getM00() * vertex.getX() + rotationMatrix.getM01() * vertex.getY(); double y = rotationMatrix.getM10() * vertex.getX() + rotationMatrix.getM11() * vertex.getY(); double z = rotationMatrix.getM20() * vertex.getX() + rotationMatrix.getM21() * vertex.getY(); vertex.set(x, y, z); } for (Vector3D32 normal : normals) { double x = rotationMatrix.getM00() * normal.getX() + rotationMatrix.getM01() * normal.getY(); double y = rotationMatrix.getM10() * normal.getX() + rotationMatrix.getM11() * normal.getY(); double z = rotationMatrix.getM20() * normal.getX() + rotationMatrix.getM21() * normal.getY(); normal.set(x, y, z); } }
@Override public void receivedPacket(RobotConfigurationData packet) { latestRobotConfigurationData = packet; FloatingJointBasics rootJoint = fullRobotModel.getRootJoint(); TFloatArrayList newJointAngles = packet.getJointAngles(); TFloatArrayList newJointVelocities = packet.getJointAngles(); TFloatArrayList newJointTorques = packet.getJointTorques(); OneDoFJointBasics[] oneDoFJoints = fullRobotModel.getOneDoFJoints(); for (int i = 0; i < newJointAngles.size(); i++) { oneDoFJoints[i].setQ(newJointAngles.get(i)); oneDoFJoints[i].setQd(newJointVelocities.get(i)); oneDoFJoints[i].setTau(newJointTorques.get(i)); } pelvisTranslationFromRobotConfigurationData.set(packet.getRootTranslation()); pelvisOrientationFromRobotConfigurationData.set(packet.getRootOrientation()); rootJoint.getJointPose().setPosition(pelvisTranslationFromRobotConfigurationData.getX(), pelvisTranslationFromRobotConfigurationData.getY(), pelvisTranslationFromRobotConfigurationData.getZ()); rootJoint.getJointPose().getOrientation().setQuaternion(pelvisOrientationFromRobotConfigurationData.getX(), pelvisOrientationFromRobotConfigurationData.getY(), pelvisOrientationFromRobotConfigurationData.getZ(), pelvisOrientationFromRobotConfigurationData.getS()); computeDriftTransform(); rootJoint.getPredecessor().updateFramesRecursively(); yoVariableServer.update(System.currentTimeMillis()); }