/** * Adds to the force part of the spatial force vector */ public void addLinearPart(Vector3d linearPart) { this.linearPart.add(linearPart); }
/** * Adds to the angular part of the spatial force vector */ public void addAngularPart(Vector3d angularPart) { this.angularPart.add(angularPart); }
public void addLinearPart(FrameVector linearPart) { expressedInFrame.checkReferenceFrameMatch(linearPart); this.linearPart.add(linearPart.getVector()); }
public static void integrateAccelerations(SixDoFJoint rootJoint, double dt) { SpatialAccelerationVector deltaTwist = new SpatialAccelerationVector(); rootJoint.getJointAcceleration(deltaTwist); deltaTwist.scale(dt); Twist rootJointTwist = new Twist(); rootJoint.getJointTwist(rootJointTwist); rootJointTwist.angularPart.add(deltaTwist.angularPart); rootJointTwist.linearPart.add(deltaTwist.linearPart); rootJoint.setJointTwist(rootJointTwist); }
public static SquaredUpDRCDemo01RobotAtPlatformsInitialSetup createInitialSetupAtNthPlatformToesTouching(int nthPlatform) { double alpha = ((double) nthPlatform)/7.0; Vector3d startingLocation = morph(firstPlatform, lastPlatform, alpha); startingLocation.add(new Vector3d(-0.11, -0.16, 0.0)); return new SquaredUpDRCDemo01RobotAtPlatformsInitialSetup(startingLocation, yaw); }
public static SquaredUpDRCDemo01RobotAtPlatformsInitialSetup createInitialSetupAtNthWall(int nthWall) { double alpha = ((double) nthWall)/7.0; Vector3d startingLocation = morph(firstPlatform, lastPlatform, alpha); startingLocation.add(new Vector3d(-1.0 - 0.1, 1.0 - 0.1, 0.0)); return new SquaredUpDRCDemo01RobotAtPlatformsInitialSetup(startingLocation, yaw - Math.PI/2.0); }
public static SquaredUpDRCDemo01RobotAtPlatformsInitialSetup createInitialSetupAtNthWall(int nthWall) { double alpha = ((double) nthWall)/7.0; Vector3d startingLocation = morph(firstPlatform, lastPlatform, alpha); startingLocation.add(new Vector3d(-1.0 - 0.1, 1.0 - 0.1, 0.0)); return new SquaredUpDRCDemo01RobotAtPlatformsInitialSetup(startingLocation, yaw - Math.PI/2.0); }
public static SquaredUpDRCDemo01RobotAtPlatformsInitialSetup createInitialSetupAtNthPlatformToesTouching(int nthPlatform) { double alpha = ((double) nthPlatform)/7.0; Vector3d startingLocation = morph(firstPlatform, lastPlatform, alpha); startingLocation.add(new Vector3d(-0.11, -0.16, 0.0)); return new SquaredUpDRCDemo01RobotAtPlatformsInitialSetup(startingLocation, yaw); }
public static Vector3d morph(Vector3d point1, Vector3d point2, double alpha) { Vector3d framePoint1 = new Vector3d(point1); Vector3d framePoint2 = new Vector3d(point2); framePoint1.scale(1.0 - alpha); framePoint2.scale(alpha); framePoint1.add(framePoint2); return framePoint1; }
public static EarthVector translateDegrees( final double lat, final double lon, final Vector3d translation) { final EarthVector result = EarthVector.fromDegrees(lat, lon); result.getVector().add(translation); return new EarthVector(result.getVector()); }
public static Vector3d morph(Vector3d point1, Vector3d point2, double alpha) { Vector3d framePoint1 = new Vector3d(point1); Vector3d framePoint2 = new Vector3d(point2); framePoint1.scale(1.0 - alpha); framePoint2.scale(alpha); framePoint1.add(framePoint2); return framePoint1; }
public static Vector3d morph(Vector3d point1, Vector3d point2, double alpha) { Vector3d framePoint1 = new Vector3d(point1); Vector3d framePoint2 = new Vector3d(point2); framePoint1.scale(1.0 - alpha); framePoint2.scale(alpha); framePoint1.add(framePoint2); return framePoint1; }
public static Vector3d morph(Vector3d point1, Vector3d point2, double alpha) { Vector3d framePoint1 = new Vector3d(point1); Vector3d framePoint2 = new Vector3d(point2); framePoint1.scale(1.0 - alpha); framePoint2.scale(alpha); framePoint1.add(framePoint2); return framePoint1; }
@Override protected void orthogonalProjectionShapeFrame(Point3d pointToCheckAndPack) { computeCompositeVectorsForPoint(originToRadiusTemporaryVector, tubeCenterToPointTemporaryVector, pointToCheckAndPack); if (tubeCenterToPointTemporaryVector.length() > tubeRadius) tubeCenterToPointTemporaryVector.scale(tubeRadius / tubeCenterToPointTemporaryVector.length()); tubeCenterToPointTemporaryVector.add(originToRadiusTemporaryVector); pointToCheckAndPack.set(tubeCenterToPointTemporaryVector); }
public void computeAngularAcceleration(Quat4d q, Quat4d qDot, Quat4d qDDot, Vector3d angularAccelerationToPack) { qConj.conjugate(q); qDotConj.conjugate(qDot); multiply(qDot, qDotConj, intermediateAngularAcceleration); multiply(qDDot, qConj, angularAccelerationToPack); angularAccelerationToPack.add(intermediateAngularAcceleration); angularAccelerationToPack.scale(2.0); }
private static void setTranslationSettingZAndPreservingXAndY(Point3d highestVertex, RigidBodyTransform transformToReturn) { Vector3d newTranslation = new Vector3d(highestVertex.getX(), highestVertex.getY(), 0.0); transformToReturn.transform(newTranslation); newTranslation.scale(-1.0); newTranslation.add(highestVertex); transformToReturn.setTranslation(newTranslation); }
public void setIncludingFrame(FrameVector force, FrameVector moment, FramePoint pointOfApplication) { force.checkReferenceFrameMatch(pointOfApplication); expressedInFrame = force.getReferenceFrame(); force.get(linearPart); pointOfApplication.get(tempVector); angularPart.cross(tempVector, linearPart); angularPart.add(moment.getVector()); }
/** * Sets camera position and rotation. * * @param pGlu * GLU */ private void setCamera(GLU pGlu) { Point3d pos = this.simpleMoveAnimator.getPoint(); Vector3d posLookAt = new Vector3d(100, 0, 0); Vector3d rotate = this.simpleMoveAnimator.getAngle(); posLookAt = PointUtil.rotateZ3d(posLookAt, rotate.z); posLookAt = PointUtil.rotateY3d(posLookAt, rotate.y); posLookAt.add(pos); pGlu.gluLookAt(pos.x, pos.y, pos.z, posLookAt.x, posLookAt.y, posLookAt.z, 0, 1, 0); }
private void setColumn(Twist twist, FramePoint comPositionScaledByMass, double subTreeMass, int column) { tempVector.set(comPositionScaledByMass.getPoint()); tempVector.cross(twist.getAngularPart(), tempVector); tempJacobianColumn.set(tempVector); tempVector.set(twist.getLinearPart()); tempVector.scale(subTreeMass); tempJacobianColumn.add(tempVector); jacobianMatrix.set(0, column, tempJacobianColumn.getX()); jacobianMatrix.set(1, column, tempJacobianColumn.getY()); jacobianMatrix.set(2, column, tempJacobianColumn.getZ()); } }