/**
* Rotate the point xyz around the line passing through abc with direction uvw
* http://inside.mines.edu/~gmurray/ArbitraryAxisRotation/ArbitraryAxisRotation.html Special case
* where abc=0
*
* @param vec
* @param axis
* @param angle_radians in radians
* @return
*/
public static Vector3f rotateAroundAxis(Vector3f vec, Vector3f axis, double angle_radians) {
float C = (float) Math.cos(angle_radians);
float S = (float) Math.sin(angle_radians);
float x = vec.x;
float y = vec.y;
float z = vec.z;
float u = axis.x;
float v = axis.y;
float w = axis.z;
// (a*( v*v + w*w) - u*(b*v + c*w - u*x - v*y - w*z))(1.0-C)+x*C+(-c*v + b*w - w*y + v*z)*S
// (b*( u*u + w*w) - v*(a*v + c*w - u*x - v*y - w*z))(1.0-C)+y*C+( c*u - a*w + w*x - u*z)*S
// (c*( u*u + v*v) - w*(a*v + b*v - u*x - v*y - w*z))(1.0-C)+z*C+(-b*u + a*v - v*x + u*y)*S
// but a=b=c=0 so
// x' = ( -u*(- u*x - v*y - w*z)) * (1.0-C) + x*C + ( - w*y + v*z)*S
// y' = ( -v*(- u*x - v*y - w*z)) * (1.0-C) + y*C + ( + w*x - u*z)*S
// z' = ( -w*(- u*x - v*y - w*z)) * (1.0-C) + z*C + ( - v*x + u*y)*S
float a = (-u * x - v * y - w * z);
return new Vector3f(
(-u * a) * (1.0f - C) + x * C + (-w * y + v * z) * S,
(-v * a) * (1.0f - C) + y * C + (w * x - u * z) * S,
(-w * a) * (1.0f - C) + z * C + (-v * x + u * y) * S);
}
public void rotateFinger() {
Vector3f forward = new Vector3f(HOME_FORWARD_X, HOME_FORWARD_Y, HOME_FORWARD_Z);
Vector3f right = new Vector3f(HOME_RIGHT_X, HOME_RIGHT_Y, HOME_RIGHT_Z);
Vector3f up = new Vector3f();
up.cross(forward, right);
Vector3f of = new Vector3f(forward);
Vector3f or = new Vector3f(right);
Vector3f ou = new Vector3f(up);
Vector3f result;
result =
rotateAroundAxis(
forward,
of,
Math.toRadians(motion_future.iku)); // TODO rotating around itself has no effect.
result = rotateAroundAxis(result, or, Math.toRadians(motion_future.ikv));
result = rotateAroundAxis(result, ou, Math.toRadians(motion_future.ikw));
motion_future.finger_forward.set(result);
result = rotateAroundAxis(right, of, Math.toRadians(motion_future.iku));
result = rotateAroundAxis(result, or, Math.toRadians(motion_future.ikv));
result = rotateAroundAxis(result, ou, Math.toRadians(motion_future.ikw));
motion_future.finger_left.set(result);
motion_future.finger_up.cross(motion_future.finger_forward, motion_future.finger_left);
}
public RotaryStewartPlatform2() {
super();
setDisplayName("Rotary Stewart Platform 2");
/*
// set up bounding volumes
for(int i=0;i<volumes.length;++i) {
volumes[i] = new Cylinder();
}
volumes[0].radius=3.2f;
volumes[1].radius=3.0f*0.575f;
volumes[2].radius=2.2f;
volumes[3].radius=1.15f;
volumes[4].radius=1.2f;
volumes[5].radius=1.0f*0.575f;*/
motion_now.rotateBase(0, 0);
motion_now.updateIKEndEffector();
motion_now.rebuildShoulders();
motion_now.updateIKWrists();
motion_future.set(motion_now);
setupModels();
// find the starting height of the end effector at home position
// @TODO: project wrist-on-bicep to get more accurate distance
float aa = (motion_now.arms[0].elbow.y - motion_now.arms[0].wrist.y);
float cc = FOREARM_LENGTH;
float bb = (float) Math.sqrt((cc * cc) - (aa * aa));
aa = motion_now.arms[0].elbow.x - motion_now.arms[0].wrist.x;
cc = bb;
bb = (float) Math.sqrt((cc * cc) - (aa * aa));
motion_now.finger_tip.set(0, 0, bb + BASE_TO_SHOULDER_Z - WRIST_TO_FINGER_Z);
motion_future.finger_tip.set(motion_now.finger_tip);
moveIfAble();
}