// 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 ofQuaternion::makeRotate_original( const ofVec3f& from, const ofVec3f& 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. ofVec3f 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); ofVec3f fromd(from.x, from.y, from.z); // find orthogonal axis. ofVec3f axis(fromd.getCrossed(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. ofVec3f axis(from.getCrossed(to)); float angle = acos( cosangle ); makeRotate( angle, axis ); } }
void findCoordinateSystem(vector<ofVec3f> &input,ofVec3f &origin,ofVec3f &xVec,ofVec3f &yVec) { origin = input[0]; xVec = (input[1]-input[0]).getNormalized(); float proj = xVec.dot(input[3]-input[0]); yVec = (input[3]-(input[0]+xVec*proj)).getNormalized(); }