/**
* Rotate the oriented bounding box of the 3D image about the specified axis with the specified
* angle.
*
* @param transform The transform and its matrix by which to rotate the image.
*/
public void rotateFrameBy(Transform3D transform) {
double rY, rX, rZ;
double sinrX, sinrY, sinrZ, cosrX, cosrY, cosrZ;
Matrix3d matrix = new Matrix3d();
transform.get(matrix);
rY = -Math.asin(matrix.m02);
if (Math.cos(rY) != 0) {
rX = -Math.atan2(matrix.m12, matrix.m22);
rZ = Math.atan2(matrix.m01, matrix.m00);
} else {
rX = -Math.atan2(matrix.m10, matrix.m11);
rZ = 0;
}
cosrX = Math.cos(rX);
sinrX = Math.sin(rX);
cosrY = Math.cos(rY);
sinrY = Math.sin(rY);
cosrZ = Math.cos(rZ);
sinrZ = Math.sin(rZ);
matrix.m00 = cosrZ * cosrY;
matrix.m01 = -sinrZ * cosrY;
matrix.m02 = sinrY;
matrix.m10 = (cosrZ * sinrY * sinrX) + (sinrZ * cosrX);
matrix.m11 = (-sinrZ * sinrY * sinrX) + (cosrZ * cosrX);
matrix.m12 = -cosrY * sinrX;
matrix.m20 = (-cosrZ * sinrY * cosrX) + (sinrZ * sinrX);
matrix.m21 = (sinrZ * sinrY * cosrX) + (cosrZ * sinrX);
matrix.m22 = cosrY * cosrX;
m_kRotate.set(matrix);
m_akAxis[0] = new Vector3f(1.0f, 0.0f, 0.0f);
m_akAxis[1] = new Vector3f(0.0f, 1.0f, 0.0f);
m_akAxis[2] = new Vector3f(0.0f, 0.0f, 1.0f);
for (int i = 0; i < 3; i++) {
m_kRotate.transform(m_akAxis[i]);
}
orthonormalize(m_akAxis);
for (int i = 0; i < 3; i++) {
setAxis(i, m_akAxis[i]);
}
}
/**
* resolve
*
* <p>マウスの動きから生成された T3D を逆運動学サーバを使って 各ジョイントの動きに直し設定する
*
* @param transform
* @return
*/
public boolean resolve(Transform3D transform) {
_setLinkStatus(robot_.getName());
Matrix3d m3d = new Matrix3d();
Vector3d v3d = new Vector3d();
transform.get(m3d, v3d);
v3d.get(tr.p);
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
tr.R[3 * i + j] = m3d.getElement(i, j);
}
}
try {
if (robot_ == null || from_ == null || to_ == null) return false;
if (!integrator_.calcCharacterInverseKinematics(
robot_.getName(), from_.getName(), to_.getName(), tr)) {
System.out.println("ik failed.");
robot_.calcForwardKinematics();
return false;
}
;
} catch (Exception e) {
e.printStackTrace();
}
DblSequenceHolder v = new DblSequenceHolder();
integrator_.getCharacterAllLinkData(robot_.getName(), LinkDataType.JOINT_VALUE, v);
robot_.setJointValues(v.value);
robot_.setJointValuesWithinLimit();
robot_.updateInitialJointValues();
_setRootJoint(robot_);
robot_.calcForwardKinematics();
return true;
}
