public void computeQDot(QuaternionReadOnly q, Vector3DReadOnly angularVelocity, Vector4DBasics qDotToPack) { multiply(angularVelocity, q, qDotToPack); qDotToPack.scale(0.5); }
public void computeQDDot(QuaternionReadOnly q, Vector4DReadOnly qDot, Vector3DReadOnly angularVelocity, Vector3DReadOnly angularAcceleration, Vector4DBasics qDDotToPack) { multiply(angularAcceleration, q, intermediateQDDot); multiply(angularVelocity, qDot, qDDotToPack); qDDotToPack.add(intermediateQDDot); qDDotToPack.scale(0.5); }
public void computeQDot(Quat4d q, Vector3d angularVelocity, Quat4d qDotToPack) { multiply(angularVelocity, q, qDotToPack); qDotToPack.scale(0.5); }
public void computeAngularVelocityInBodyFixedFrame(QuaternionReadOnly q, Vector4DReadOnly qDot, Vector3DBasics angularVelocityToPack) { qConj.setAndConjugate(q); multiply(qConj, qDot, angularVelocityToPack); angularVelocityToPack.scale(2.0); }
public void computeAngularVelocityInBodyFixedFrame(Quat4d q, Quat4d qDot, Vector3d angularVelocityToPack) { qConj.conjugate(q); multiply(qConj, qDot, angularVelocityToPack); angularVelocityToPack.scale(2.0); }
public void computeAngularVelocityInWorldFrame(Quat4d q, Quat4d qDot, Vector3d angularVelocityToPack) { qConj.conjugate(q); multiply(qDot, qConj, angularVelocityToPack); angularVelocityToPack.scale(2.0); }
public void computeAngularVelocityInWorldFrame(QuaternionReadOnly q, Vector4DReadOnly qDot, Vector3DBasics angularVelocityToPack) { qConj.setAndConjugate(q); multiply(qDot, qConj, angularVelocityToPack); angularVelocityToPack.scale(2.0); }
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); }
public void computeAngularAcceleration(QuaternionReadOnly q, Vector4DReadOnly qDot, Vector4DReadOnly qDDot, Vector3DBasics angularAccelerationToPack) { qConj.setAndConjugate(q); qDotConj.set(-qDot.getX(), -qDot.getY(), -qDot.getZ(), qDot.getS()); multiply(qDot, qDotConj, intermediateAngularAcceleration); multiply(qDDot, qConj, angularAccelerationToPack); angularAccelerationToPack.add(intermediateAngularAcceleration); angularAccelerationToPack.scale(2.0); }