Ejemplo n.º 1
0
// 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 );
    }
}
Ejemplo n.º 2
0
// 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);
    }
}