public void setPositionAndVelocity(Tuple3d position, Tuple3d velocity) {
q_x.set(position.getX());
q_y.set(position.getY());
q_z.set(position.getZ());
qd_x.set(velocity.getX());
qd_y.set(velocity.getY());
qd_z.set(velocity.getZ());
}
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());
}
public void setAcceleration(Tuple3d acceleration) {
qdd_x.set(acceleration.getX());
qdd_y.set(acceleration.getY());
qdd_z.set(acceleration.getZ());
}
public void setVelocity(Tuple3d velocity) {
qd_x.set(velocity.getX());
qd_y.set(velocity.getY());
qd_z.set(velocity.getZ());
}
public void setPosition(Tuple3d position) {
q_x.set(position.getX());
q_y.set(position.getY());
q_z.set(position.getZ());
}