public void getVelocity(Tuple3d velocity) { velocity.set(qd_x.getDoubleValue(), qd_y.getDoubleValue(), qd_z.getDoubleValue()); }
public void getPosition(Tuple3d position) { position.set(q_x.getDoubleValue(), q_y.getDoubleValue(), q_z.getDoubleValue()); }