public void setYawPitchRoll(double yaw, double pitch, double roll) {
Quat4d q = new Quat4d();
RotationTools.convertYawPitchRollToQuaternion(yaw, pitch, roll, q);
RotationTools.checkQuaternionNormalized(q);
q_qs.set(q.getW());
q_qx.set(q.getX());
q_qy.set(q.getY());
q_qz.set(q.getZ());
}
public static Vector3d quatRotate(Quat4d rotation, Vector3d v, Vector3d out) {
Quat4d q = new Quat4d(rotation);
QuaternionUtil.mul(q, v);
Quat4d tmp = new Quat4d();
inverse(tmp, rotation);
q.mul(tmp);
out.set(q.x, q.y, q.z);
return out;
}
public static void setEuler(Quat4d q, double yaw, double pitch, double roll) {
double halfYaw = yaw * 0.5f;
double halfPitch = pitch * 0.5f;
double halfRoll = roll * 0.5f;
double cosYaw = (double) Math.cos(halfYaw);
double sinYaw = (double) Math.sin(halfYaw);
double cosPitch = (double) Math.cos(halfPitch);
double sinPitch = (double) Math.sin(halfPitch);
double cosRoll = (double) Math.cos(halfRoll);
double sinRoll = (double) Math.sin(halfRoll);
q.x = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
q.y = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
q.z = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
q.w = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;
}
// private Matrix3d tempRotationMatrix = new Matrix3d();
protected void setFloatingTransform3D(RigidBodyTransform t1) {
// position.set(q_x.val + offset.x, q_y.val + offset.y, q_z.val + offset.z);
tempPosition1.set(q_x.getDoubleValue(), q_y.getDoubleValue(), q_z.getDoubleValue());
tempOrientation1.set(
q_qx.getDoubleValue(), q_qy.getDoubleValue(), q_qz.getDoubleValue(), q_qs.getDoubleValue());
t1.set(tempOrientation1, tempPosition1);
// // An alternate way is to hardcode from
// http://www.genesis3d.com/~kdtop/Quaternions-UsingToRepresentRotation.htm (except do the
// transpose):
// double w2 = q_qs.val * q_qs.val;
// double x2 = q_qx.val * q_qx.val;
// double y2 = q_qy.val * q_qy.val;
// double z2 = q_qz.val * q_qz.val;
//
// double wx = q_qs.val * q_qx.val;
// double wy = q_qs.val * q_qy.val;
// double wz = q_qs.val * q_qz.val;
//
// double xy = q_qx.val * q_qy.val;
// double xz = q_qx.val * q_qz.val;
// double yz = q_qy.val * q_qz.val;
//
//
//// Matrix3d rotationMatrix = new Matrix3d
//
// tempRotationMatrix = new Matrix3d(
// w2+x2-y2-z2, 2.0*(xy - wz), 2.0*(xz + wy),
// 2.0*(xy+wz), w2-x2+y2-z2, 2.0*(yz-wx),
// 2.0*(xz-wy), 2.0*(yz+wx), w2-x2-y2+z2
// );
//
// t1.set(tempRotationMatrix, tempPosition1, 1.0);
}
public static void mul(Quat4d q, Vector3d w) {
double rx = q.w * w.x + q.y * w.z - q.z * w.y;
double ry = q.w * w.y + q.z * w.x - q.x * w.z;
double rz = q.w * w.z + q.x * w.y - q.y * w.x;
double rw = -q.x * w.x - q.y * w.y - q.z * w.z;
q.set(rx, ry, rz, rw);
}
// Game Programming Gems 2.10. make sure v0,v1 are normalized
public static Quat4d shortestArcQuat(Vector3d v0, Vector3d v1, Quat4d out) {
Vector3d c = new Vector3d();
c.cross(v0, v1);
double d = v0.dot(v1);
if (d < -1.0 + BulletGlobals.FLT_EPSILON) {
// just pick any vector
out.set(0.0f, 1.0f, 0.0f, 0.0f);
return out;
}
double s = (double) Math.sqrt((1.0f + d) * 2.0f);
double rs = 1.0f / s;
out.set(c.x * rs, c.y * rs, c.z * rs, s * 0.5f);
return out;
}
protected void jointDependentSetAndGetRotation(Matrix3d Rh_i) {
tempOrientation2.set(
owner.q_qx.getDoubleValue(),
owner.q_qy.getDoubleValue(),
owner.q_qz.getDoubleValue(),
owner.q_qs.getDoubleValue());
Rh_i.set(tempOrientation2);
}
public void getQuaternion(Quat4d quaternionToPack) {
quaternionToPack.set(
q_qx.getDoubleValue(), q_qy.getDoubleValue(), q_qz.getDoubleValue(), q_qs.getDoubleValue());
}
public void setQuaternion(Quat4d q) {
q_qs.set(q.getW());
q_qx.set(q.getX());
q_qy.set(q.getY());
q_qz.set(q.getZ());
}
public static void inverse(Quat4d q, Quat4d src) {
q.x = -src.x;
q.y = -src.y;
q.z = -src.z;
q.w = src.w;
}
public static void inverse(Quat4d q) {
q.x = -q.x;
q.y = -q.y;
q.z = -q.z;
}
public static void setRotation(Quat4d q, Vector3d axis, double angle) {
double d = axis.length();
assert (d != 0f);
double s = (double) Math.sin(angle * 0.5f) / d;
q.set(axis.x * s, axis.y * s, axis.z * s, (double) Math.cos(angle * 0.5f));
}
private static void packQuat4dToGeometry_msgsQuaternion(Quat4d quat, Quaternion orientation) {
orientation.setW(quat.getW());
orientation.setX(quat.getX());
orientation.setY(quat.getY());
orientation.setZ(quat.getZ());
}
public static void packRosQuaternionToQuat4d(Quaternion rosQuat, Quat4d quat) {
quat.setX(rosQuat.getX());
quat.setY(rosQuat.getY());
quat.setZ(rosQuat.getZ());
quat.setW(rosQuat.getW());
}
@Override
@SuppressWarnings("rawtypes")
public void processStimulus(Enumeration criteria) { // examiner criteria �
while (criteria.hasMoreElements()) {
WakeupOnAWTEvent w = (WakeupOnAWTEvent) criteria.nextElement();
AWTEvent events[] = w.getAWTEvent();
synchronized (deltaT) {
synchronized (deltaR) {
for (int i = 0; i < events.length; i++) {
if (events[i].getID() == KeyEvent.KEY_PRESSED) {
int k = ((KeyEvent) events[i]).getKeyCode();
if (k == KeyEvent.VK_UP) {
deltaT.z = -0.1;
} else if (k == KeyEvent.VK_DOWN) {
deltaT.z = 0.1;
} else if (k == KeyEvent.VK_PAGE_UP) {
deltaT.y = 0.1;
} else if (k == KeyEvent.VK_PAGE_DOWN) {
deltaT.y = -0.1;
} else if (k == KeyEvent.VK_LEFT) {
deltaR.set(new AxisAngle4d(new Vector3d(0, 1, 0), 0.02));
} else if (k == KeyEvent.VK_RIGHT) {
deltaR.set(new AxisAngle4d(new Vector3d(0, 1, 0), -0.02));
}
} else if (events[i].getID() == KeyEvent.KEY_RELEASED) {
int k = ((KeyEvent) events[i]).getKeyCode();
if (k == KeyEvent.VK_UP) {
deltaT.z = 0;
} else if (k == KeyEvent.VK_DOWN) {
deltaT.z = 0;
} else if (k == KeyEvent.VK_PAGE_UP) {
deltaT.y = 0;
} else if (k == KeyEvent.VK_PAGE_DOWN) {
deltaT.y = 0;
} else if (k == KeyEvent.VK_LEFT) {
deltaR.set(new AxisAngle4d(new Vector3d(0, 1, 0), 0.0));
} else if (k == KeyEvent.VK_RIGHT) {
deltaR.set(new AxisAngle4d(new Vector3d(0, 1, 0), 0.0));
}
} else if (events[i].getID() == MouseEvent.MOUSE_WHEEL) {
MouseWheelEvent mwe = ((MouseWheelEvent) events[i]);
// deltaT.y = deltaT.y + mwe.getPreciseWheelRotation () / 10 ;
// } else if ((events [i].getID () == MouseEvent.MOUSE_DRAGGED) ||
// (events [i].getID () == MouseEvent.MOUSE_PRESSED)) {
// MouseEvent me = ((MouseEvent)events [i]) ;
// int width = me.getComponent ().getWidth () ;
// int height = me.getComponent ().getHeight () ;
// int x = me.getX () ;
// int y = me.getY () ;
// double azimuth = Math.atan2 (width / 2.0 - x, 500.0) ;
// double elevation = Math.atan2 (height / 2.0 - y, 500.0) ;
// Quat4d azimuthR = new Quat4d () ;
// azimuthR.set (new AxisAngle4d (0, 1, 0, azimuth)) ;
// Quat4d elevationR = new Quat4d () ;
// elevationR.set (new AxisAngle4d (1, 0, 0, elevation)) ;
// absoluteR.mul (azimuthR, elevationR) ;
// navigator.setHeadOrientationInSupportFrame (absoluteR.x,
// absoluteR.y, absoluteR.z, absoluteR.w);
} else if (events[i].getID() == MouseEvent.MOUSE_PRESSED) {
initHeadR = navigator.getHeadRotationInSupportFrame();
MouseEvent me = ((MouseEvent) events[i]);
xInit = me.getX();
yInit = me.getY();
} else if (events[i].getID() == MouseEvent.MOUSE_DRAGGED) {
MouseEvent me = ((MouseEvent) events[i]);
int dx = me.getX() - xInit;
int dy = me.getY() - yInit;
double azimuth = Math.atan2(-dx, 500.0);
double elevation = Math.atan2(-dy, 500.0);
Quat4d azimuthR = new Quat4d();
azimuthR.set(new AxisAngle4d(0, 1, 0, azimuth));
Quat4d elevationR = new Quat4d();
elevationR.set(new AxisAngle4d(1, 0, 0, elevation));
Quat4d relativeR = new Quat4d();
relativeR.mul(azimuthR, elevationR);
absoluteR.mul(initHeadR, relativeR);
navigator.setHeadOrientationInSupportFrame(
absoluteR.x, absoluteR.y, absoluteR.z, absoluteR.w);
}
}
}
}
wakeupOn(wEvents);
}
}
