public void getRay(Vector3D rayToPack, int rayIndex) { MathTools.checkIntervalContains(rayIndex, 0, numberOfRays - 1); rayToPack.sub(sphereOrigin, pointsOnSphere[rayIndex]); rayToPack.normalize(); }
private Vector3D getDirection(Point3D a, Point3D b) { Vector3D direction = new Vector3D(b); direction.sub(a); direction.scale(0.5); return direction; }
public double orthogonalDistance(Point3D point) { temporaryVector.set(point); temporaryVector.sub(this.point); return temporaryVector.dot(normal); }
public boolean isInVoronoiRegionOfVertex(Point3D pointToCheck, Point3D otherPoint1, Point3D otherPoint2) { tempVector1.set(pointToCheck); tempVector1.scale(-1.0); tempVector2.sub(otherPoint1, pointToCheck); if (!(tempVector1.dot(tempVector2) <= 0.0)) return false; tempVector2.sub(otherPoint2, pointToCheck); if (!(tempVector1.dot(tempVector2) <= 0.0)) return false; return true; }
public double computeTripleProductIfTetragon() { if (pointFour == null) return Double.NaN; Vector3D vectorAB = new Vector3D(); Vector3D vectorAC = new Vector3D(); Vector3D vectorAD = new Vector3D(); Vector3D normalVector = new Vector3D(); vectorAB.sub(pointTwo, pointOne); vectorAC.sub(pointThree, pointOne); vectorAD.sub(pointFour, pointOne); normalVector.cross(vectorAB, vectorAC); double tripleProduct = vectorAD.dot(normalVector); return tripleProduct; }
public boolean isInVoronoiRegionOfVertex(Point3D pointToCheck, Point3D otherPoint) { tempVector1.set(pointToCheck); tempVector1.scale(-1.0); tempVector2.sub(otherPoint, pointToCheck); return (tempVector1.dot(tempVector2) <= 0.0); }
private void updatePointsAndVectors(Point3DReadOnly initialPosition, Point3DReadOnly finalPosition, double velocityMagnitude) { this.finalPosition.set(finalPosition); directionVector.set(finalPosition); directionVector.sub(initialPosition); directionVector.normalize(); velocityVector.set(directionVector); velocityVector.scale(velocityMagnitude); }
public double orhtogonalDistance(NormalOcTreeNode node) { temporaryVector.set(node.getHitLocationX(), node.getHitLocationY(), node.getHitLocationZ()); temporaryVector.sub(point); return temporaryVector.dot(normal); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { eX.sub(p2, p1); eX.normalize(); eY.sub(p3, p1); // temp only eZ.cross(eX, eY); eZ.normalize(); eY.cross(eZ, eX); rotation.setColumns(eX, eY, eZ); transformToParent.setRotationAndZeroTranslation(rotation); transformToParent.setTranslation(p1.getX(), p1.getY(), p1.getZ()); } }
private void computeInitialConstraintError(FramePoint3DReadOnly initialPosition, double initialTime) { trajectory.compute(initialTime); trajectoryFrame.checkReferenceFrameMatch(initialPosition.getReferenceFrame()); trajectory.getPosition(tempPosition); tempPosition.changeFrame(trajectoryFrame); initialConstraintPositionError.set(initialPosition); initialConstraintPositionError.sub(tempPosition); }
private void computeFinalConstraintError(FramePoint3DReadOnly finalPosition, FrameVector3DReadOnly finalVelocity, double finalTime) { computeFinalConstraintError(finalPosition, finalTime); trajectoryFrame.checkReferenceFrameMatch(finalVelocity.getReferenceFrame()); trajectory.getVelocity(tempVelocity); tempVelocity.changeFrame(trajectoryFrame); finalConstraintVelocityError.set(finalVelocity); finalConstraintVelocityError.sub(tempVelocity); }
private void computeInitialConstraintError(FramePoint3DReadOnly initialPosition, FrameVector3DReadOnly initialVelocity, double initialTime) { computeInitialConstraintError(initialPosition, initialTime); trajectoryFrame.checkReferenceFrameMatch(initialVelocity.getReferenceFrame()); trajectory.getVelocity(tempVelocity); tempVelocity.changeFrame(trajectoryFrame); initialConstraintVelocityError.set(initialVelocity); initialConstraintVelocityError.sub(tempVelocity); }
private void computeFinalConstraintError(FramePoint3DReadOnly finalPosition, double finalTime) { trajectory.compute(finalTime); trajectoryFrame.checkReferenceFrameMatch(finalPosition.getReferenceFrame()); trajectory.getPosition(tempPosition); tempPosition.changeFrame(trajectoryFrame); finalConstraintPositionError.set(finalPosition); finalConstraintPositionError.sub(tempPosition); }
private void computeFinalConstraintError(FrameQuaternionReadOnly finalOrientation, FrameVector3DReadOnly finalAngularVelocity, double finalTime) { computeFinalConstraintError(finalOrientation, finalTime); trajectoryFrame.checkReferenceFrameMatch(finalAngularVelocity.getReferenceFrame()); trajectory.getAngularVelocity(tempAngularVelocity); tempAngularVelocity.changeFrame(trajectoryFrame); finalConstraintAngularVelocityError.set(finalAngularVelocity); finalConstraintAngularVelocityError.sub(tempAngularVelocity); }
private void computeInitialConstraintError(FrameQuaternionReadOnly initialOrientation, FrameVector3DReadOnly initialAngularVelocity, double initialTime) { computeInitialConstraintError(initialOrientation, initialTime); trajectoryFrame.checkReferenceFrameMatch(initialAngularVelocity.getReferenceFrame()); trajectory.getAngularVelocity(tempAngularVelocity); tempAngularVelocity.changeFrame(trajectoryFrame); initialConstraintAngularVelocityError.set(initialAngularVelocity); initialConstraintAngularVelocityError.sub(tempAngularVelocity); }
private void addCylinderLinkGraphics(Vector3D linkStartPointOffsetFromParentJoint, double linkThickness, Vector3D linkVectorInWorld, AppearanceDefinition color, double trimBothEndsByThisMuch) { linkVectorCopy.set(linkVectorInWorld); Graphics3DObject linkGraphics = getLinkGraphics(); linkGraphics.identity(); linkGraphics.translate(linkStartPointOffsetFromParentJoint); linkVectorCopy.sub(linkStartPointOffsetFromParentJoint); linkGraphics.rotate(EuclidGeometryTools.axisAngleFromZUpToVector3D(linkVectorCopy)); linkGraphics.translate(0.0, 0.0, trimBothEndsByThisMuch); linkGraphics.addCylinder(linkVectorCopy.length() - trimBothEndsByThisMuch, linkThickness, color); }
public void update(Tuple3DReadOnly currentPosition) { if (!hasBeenCalled.getBooleanValue()) { hasBeenCalled.set(true); lastPosition.set(currentPosition); setToZero(); } currentRawDerivative.sub(currentPosition, lastPosition); currentRawDerivative.scale(1.0 / dt); double alpha = alphaProvider.getValue(); interpolate(currentRawDerivative, this, alpha); lastPosition.set(currentPosition); } }
private void addCylinderLinkGraphics(Vector3D linkStartPointOffsetFromParentJoint, double linkThickness, Joint jointWhereLinkEnds, AppearanceDefinition color, double trimBothEndsByThisMuch) { jointWhereLinkEnds.getOffset(linkVectorCopy); Graphics3DObject linkGraphics = getLinkGraphics(); linkGraphics.identity(); linkGraphics.translate(linkStartPointOffsetFromParentJoint); linkVectorCopy.sub(linkStartPointOffsetFromParentJoint); linkGraphics.rotate(EuclidGeometryTools.axisAngleFromZUpToVector3D(linkVectorCopy)); linkGraphics.translate(0.0, 0.0, trimBothEndsByThisMuch); linkGraphics.addCylinder(linkVectorCopy.length() - trimBothEndsByThisMuch, linkThickness, color); }
public void addCylinderLinkGraphics(Point3D cylinderStart, Point3D cylinderEnd, double linkThickness, AppearanceDefinition color, double trimBothEndsByThisMuch) { Graphics3DObject linkGraphics = getLinkGraphics(); linkGraphics.identity(); Vector3D linkVector = new Vector3D(); linkVector.sub(cylinderEnd, cylinderStart); linkGraphics.translate(cylinderStart); linkGraphics.rotate(EuclidGeometryTools.axisAngleFromZUpToVector3D(linkVector)); linkGraphics.translate(0.0, 0.0, trimBothEndsByThisMuch); linkGraphics.addCylinder(linkVector.length() - trimBothEndsByThisMuch, linkThickness, color); } }
private void projectOriginOntoEdge(Point3D vertexOne, Point3D vertexTwo, Point3D projectionToPack) { tempVector1.set(vertexOne); tempVector1.scale(-1.0); tempVector2.sub(vertexTwo, vertexOne); double percentFromVertexOneToVertexTwo = tempVector1.dot(tempVector2) / (tempVector2.dot(tempVector2)); tempVector2.scale(percentFromVertexOneToVertexTwo); projectionToPack.set(vertexOne); projectionToPack.add(tempVector2); double oneMinusPercentFromVertexOneToVertexTwo = 1.0 - percentFromVertexOneToVertexTwo; lambdas.clear(); setLambda(vertexOne, oneMinusPercentFromVertexOneToVertexTwo); setLambda(vertexTwo, percentFromVertexOneToVertexTwo); }