// Make a rotation Quat which will rotate vec1 to vec2
// Generally take adot product to get the angle between these
// and then use a cross product to get the rotation axis
// Watch out for the two special cases of when the vectors
// are co-incident or opposite in direction.
void Quat::makeRotate_original( const Vec3d& from, const Vec3d& to )
{
const value_type epsilon = 0.0000001;
value_type length1 = from.length();
value_type length2 = to.length();
// dot product vec1*vec2
value_type cosangle = from*to/(length1*length2);
if ( fabs(cosangle - 1) < epsilon )
{
//OSG_INFO<<"*** Quat::makeRotate(from,to) with near co-linear vectors, epsilon= "<<fabs(cosangle-1)<<std::endl;
// cosangle is close to 1, so the vectors are close to being coincident
// Need to generate an angle of zero with any vector we like
// We'll choose (1,0,0)
makeRotate( 0.0, 0.0, 0.0, 1.0 );
}
else
if ( fabs(cosangle + 1.0) < epsilon )
{
// vectors are close to being opposite, so will need to find a
// vector orthogonal to from to rotate about.
Vec3d tmp;
if (fabs(from.x())<fabs(from.y()))
if (fabs(from.x())<fabs(from.z())) tmp.set(1.0,0.0,0.0); // use x axis.
else tmp.set(0.0,0.0,1.0);
else if (fabs(from.y())<fabs(from.z())) tmp.set(0.0,1.0,0.0);
else tmp.set(0.0,0.0,1.0);
Vec3d fromd(from.x(),from.y(),from.z());
// find orthogonal axis.
Vec3d axis(fromd^tmp);
axis.normalize();
_v[0] = axis[0]; // sin of half angle of PI is 1.0.
_v[1] = axis[1]; // sin of half angle of PI is 1.0.
_v[2] = axis[2]; // sin of half angle of PI is 1.0.
_v[3] = 0; // cos of half angle of PI is zero.
}
else
{
// This is the usual situation - take a cross-product of vec1 and vec2
// and that is the axis around which to rotate.
Vec3d axis(from^to);
value_type angle = acos( cosangle );
makeRotate( angle, axis );
}
}
// Make a rotation Quat which will rotate vec1 to vec2
// Generally take adot product to get the angle between these
// and then use a cross product to get the rotation axis
// Watch out for the two special cases of when the vectors
// are co-incident or opposite in direction.
void t3Quaternion::makeRotate_original(const t3Vector3f& from, const t3Vector3f& to)
{
const float epsilon = 0.0000001f;
float length1 = from.length();
float length2 = to.length();
// dot product vec1*vec2
float cosangle = from.dot(to) / (length1 * length2);
if(fabs(cosangle - 1) < epsilon)
{
//osg::notify(osg::INFO)<<"*** Quat::makeRotate(from,to) with near co-linear vectors, epsilon= "<<fabs(cosangle-1)<<std::endl;
// cosangle is close to 1, so the vectors are close to being coincident
// Need to generate an angle of zero with any vector we like
// We'll choose (1,0,0)
makeRotate(0.0, 0.0, 0.0, 1.0);
}
else
if(fabs(cosangle + 1.0) < epsilon)
{
// vectors are close to being opposite, so will need to find a
// vector orthongonal to from to rotate about.
t3Vector3f tmp;
if(fabs(from.x) < fabs(from.y))
if(fabs(from.x) < fabs(from.z)) tmp.set(1.0, 0.0, 0.0); // use x axis.
else tmp.set(0.0, 0.0, 1.0);
else if(fabs(from.y) < fabs(from.z)) tmp.set(0.0, 1.0, 0.0);
else tmp.set(0.0, 0.0, 1.0);
t3Vector3f fromd(from.x, from.y, from.z);
// find orthogonal axis.
t3Vector3f axis(fromd.getCrossed(tmp));
axis.normalize();
_v.x = axis[0]; // sin of half angle of PI is 1.0.
_v.y = axis[1]; // sin of half angle of PI is 1.0.
_v.z = axis[2]; // sin of half angle of PI is 1.0.
_v.w = 0; // cos of half angle of PI is zero.
}
else
{
// This is the usual situation - take a cross-product of vec1 and vec2
// and that is the axis around which to rotate.
t3Vector3f axis(from.getCrossed(to));
float angle = acos(cosangle);
makeRotate(angle, axis);
}
}
void Quat::makeRotate ( value_type angle1, const Vec3f& axis1,
value_type angle2, const Vec3f& axis2,
value_type angle3, const Vec3f& axis3)
{
makeRotate(angle1,Vec3d(axis1),
angle2,Vec3d(axis2),
angle3,Vec3d(axis3));
}
void ofApp::update(){
openNI.update();
if(openNI.isNewFrame()) {
ofxOscBundle bundle;
int deviceId = openNI.getDeviceID();
int numTrackedUsers = openNI.getNumTrackedUsers();
for(int userIndex = 0; userIndex < numTrackedUsers; userIndex++) {
ofxOpenNIUser& user = openNI.getTrackedUser(userIndex);
if(user.isTracking() && user.isSkeleton()) {
ofxOscMessage msg;
msg.setAddress("/ram/skeleton");
int userId = user.getXnID();
string actorName = "OpenNI " + ofToString(userId) + " @" + ofToString(deviceId);
int numJoints = user.getNumJoints();
msg.addStringArg(actorName);
msg.addIntArg(RAM_JOINT_COUNT);
// should use accelerometer to right things
// or a custom slider to position people
float floorOffset = 0;
vector<ofVec3f> positions(JOINT_COUNT);
for(int openniIndex = 0; openniIndex < JOINT_COUNT; openniIndex++) {
ofxOpenNIJoint& joint = user.getJoint((Joint) openniIndex);
positions[openniIndex] = joint.getWorldPosition();
positions[openniIndex].x *= -1; // openni is mirrored left/right
if(openniIndex == 0 || positions[openniIndex].y < floorOffset) {
floorOffset = positions[openniIndex].y;
}
}
vector<ofQuaternion> orientations(JOINT_COUNT);
ofVec3f torsoToNeck = positions[JOINT_NECK] - positions[JOINT_TORSO];
ofVec3f torsoToLeftHip = positions[JOINT_LEFT_HIP] - positions[JOINT_TORSO];
ofVec3f torsoToRightHip = positions[JOINT_RIGHT_HIP] - positions[JOINT_TORSO];
ofVec3f neckToTorso = positions[JOINT_TORSO] - positions[JOINT_NECK];
ofVec3f neckToRightShoulder = positions[JOINT_RIGHT_SHOULDER] - positions[JOINT_NECK];
ofVec3f headToNeck = positions[JOINT_NECK] - positions[JOINT_HEAD];
ofVec3f leftShoulderToLeftElbow = positions[JOINT_LEFT_ELBOW] - positions[JOINT_LEFT_SHOULDER];
ofVec3f leftElbowToLeftHand = positions[JOINT_LEFT_HAND] - positions[JOINT_LEFT_ELBOW];
ofVec3f rightShoulderToRightElbow = positions[JOINT_RIGHT_ELBOW] - positions[JOINT_RIGHT_SHOULDER];
ofVec3f rightElbowToRightHand = positions[JOINT_RIGHT_HAND] - positions[JOINT_RIGHT_ELBOW];
ofVec3f leftHipToLeftKnee = positions[JOINT_LEFT_KNEE] - positions[JOINT_LEFT_HIP];
ofVec3f leftKneeToLeftFoot = positions[JOINT_LEFT_FOOT] - positions[JOINT_LEFT_KNEE];
ofVec3f rightHipToRightKnee = positions[JOINT_RIGHT_KNEE] - positions[JOINT_RIGHT_HIP];
ofVec3f rightKneeToRightFoot = positions[JOINT_RIGHT_FOOT] - positions[JOINT_RIGHT_KNEE];
orientations[JOINT_TORSO] = makeRotate(torsoToNeck, torsoToRightHip);
orientations[JOINT_NECK] = makeRotate(neckToTorso, neckToRightShoulder);
orientations[JOINT_HEAD] = makeRotate(headToNeck, neckToRightShoulder);
orientations[JOINT_LEFT_SHOULDER] = makeRotate(leftShoulderToLeftElbow, neckToRightShoulder);
orientations[JOINT_LEFT_ELBOW] = makeRotate(leftElbowToLeftHand, leftShoulderToLeftElbow);
orientations[JOINT_LEFT_HAND] = orientations[JOINT_LEFT_ELBOW];
orientations[JOINT_RIGHT_SHOULDER] = makeRotate(rightShoulderToRightElbow, neckToRightShoulder);
orientations[JOINT_RIGHT_ELBOW] = makeRotate(rightElbowToRightHand, rightShoulderToRightElbow);
orientations[JOINT_RIGHT_HAND] = orientations[JOINT_RIGHT_ELBOW];
orientations[JOINT_LEFT_HIP] = makeRotate(leftHipToLeftKnee, torsoToLeftHip);
orientations[JOINT_LEFT_KNEE] = makeRotate(leftKneeToLeftFoot, leftHipToLeftKnee);
orientations[JOINT_LEFT_FOOT] = orientations[JOINT_LEFT_KNEE];
orientations[JOINT_RIGHT_HIP] = makeRotate(rightHipToRightKnee, torsoToRightHip);
orientations[JOINT_RIGHT_KNEE] = makeRotate(rightKneeToRightFoot, rightHipToRightKnee);
orientations[JOINT_RIGHT_FOOT] = orientations[JOINT_RIGHT_KNEE];
for(int ramIndex = 0; ramIndex < RAM_JOINT_COUNT; ramIndex++) {
int openniIndex = jointMapping[ramIndex];
ofVec3f position = positions[openniIndex]; // process a copy
position -= openniCenter;
position.y -= floorOffset;
position *= ramScale;
msg.addStringArg(ramJointName[ramIndex]);
msg.addFloatArg(position.x);
msg.addFloatArg(position.y);
msg.addFloatArg(position.z);
// send zero orientation
ofQuaternion& orientation = orientations[openniIndex];
ofVec3f axis;
float angle;
orientation.getRotate(angle, axis);
msg.addFloatArg(angle);
msg.addFloatArg(axis.x);
msg.addFloatArg(axis.y);
msg.addFloatArg(axis.z);
}
msg.addFloatArg(ofGetElapsedTimef());
bundle.addMessage(msg);
}
}
osc.sendBundle(bundle);
}
}
void ofQuaternion::makeRotate( float angle, const ofVec3f& vec ) {
makeRotate( angle, vec.x, vec.y, vec.z );
}
void Quat::makeRotate( value_type angle, const Vec3d& vec )
{
makeRotate( angle, vec[0], vec[1], vec[2] );
}
void Quat::makeRotate( const Vec3f& from, const Vec3f& to )
{
makeRotate( Vec3d(from), Vec3d(to) );
}
void t3Quaternion::makeRotate(float angle, const t3Vector3f& vec)
{
makeRotate(angle, vec.x, vec.y, vec.z);
}
