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 void integrateVelocities(SixDoFJoint sixDoFJoint, double dt) { Twist twist = new Twist(); sixDoFJoint.getJointTwist(twist); Matrix3d rotation = new Matrix3d(); sixDoFJoint.getRotation(rotation); Vector3d position = new Vector3d(); sixDoFJoint.getTranslation(position); integrate(rotation, position, dt, twist); sixDoFJoint.setRotation(rotation); sixDoFJoint.setPosition(position); }
public void get(double[] buffer, int offset) { inverseDynamicsJoint.getRotation(rotation); inverseDynamicsJoint.getTranslation(translation); inverseDynamicsJoint.getJointTwist(twist); buffer[offset + 0] = rotation.getW(); buffer[offset + 1] = rotation.getX(); buffer[offset + 2] = rotation.getY(); buffer[offset + 3] = rotation.getZ(); buffer[offset + 4] = translation.getX(); buffer[offset + 5] = translation.getY(); buffer[offset + 6] = translation.getZ(); twist.getArray(buffer, offset + 7); } }
public void get(double[] buffer, int offset) { inverseDynamicsJoint.getRotation(rotation); inverseDynamicsJoint.getTranslation(translation); inverseDynamicsJoint.getJointTwist(twist); buffer[offset + 0] = rotation.getW(); buffer[offset + 1] = rotation.getX(); buffer[offset + 2] = rotation.getY(); buffer[offset + 3] = rotation.getZ(); buffer[offset + 4] = translation.getX(); buffer[offset + 5] = translation.getY(); buffer[offset + 6] = translation.getZ(); twist.getArray(buffer, offset + 7); } }