Example #1
0
        public AwQuaternion(AwVector a, AwVector b)
        {
            w = 1.0;
            x = 0.0;
            y = 0.0;
            z = 0.0;

            double factor = a.length() * b.length();

            if (Math.Abs(factor) > AwMath.kFloatEpsilon)
            {
                // Vectors have length > 0
                AwVector pivotVector = new AwVector();
                double dot = a.dotProduct(b) / factor;
                double theta = Math.Acos(AwMath.clamp(dot, -1.0, 1.0));

                pivotVector = a.crossProduct(b);
                if (dot < 0.0 && pivotVector.length() < AwMath.kFloatEpsilon)
                {
                    // Vectors parallel and opposite direction, therefore a rotation
                    // of 180 degrees about any vector perpendicular to this vector
                    // will rotate vector a onto vector b.
                    //
                    // The following guarantees the dot-product will be 0.0.
                    //

                    uint dominantIndex = (uint)a.dominantAxis();
                    uint index = ( dominantIndex + 1) % 3;
                    double value = -a.getIndex( index) ;
                    pivotVector.setIndex(dominantIndex, value);
                    pivotVector.setIndex((dominantIndex + 1) % 3, a.getIndex(dominantIndex));
                    pivotVector.setIndex((dominantIndex + 2) % 3, 0);
                }
                setAxisAngle(pivotVector, theta);
            }
        }
Example #2
0
        public AwQuaternion(AwVector a, AwVector b)
        {
            w = 1.0;
            x = 0.0;
            y = 0.0;
            z = 0.0;

            double factor = a.length() * b.length();

            if (Math.Abs(factor) > AwMath.kFloatEpsilon)
            {
                // Vectors have length > 0
                AwVector pivotVector = new AwVector();
                double   dot         = a.dotProduct(b) / factor;
                double   theta       = Math.Acos(AwMath.clamp(dot, -1.0, 1.0));

                pivotVector = a.crossProduct(b);
                if (dot < 0.0 && pivotVector.length() < AwMath.kFloatEpsilon)
                {
                    // Vectors parallel and opposite direction, therefore a rotation
                    // of 180 degrees about any vector perpendicular to this vector
                    // will rotate vector a onto vector b.
                    //
                    // The following guarantees the dot-product will be 0.0.
                    //

                    uint   dominantIndex = (uint)a.dominantAxis();
                    uint   index         = (dominantIndex + 1) % 3;
                    double value         = -a.getIndex(index);
                    pivotVector.setIndex(dominantIndex, value);
                    pivotVector.setIndex((dominantIndex + 1) % 3, a.getIndex(dominantIndex));
                    pivotVector.setIndex((dominantIndex + 2) % 3, 0);
                }
                setAxisAngle(pivotVector, theta);
            }
        }
Example #3
0
        void solveIK(AwPoint startJointPos,
                     AwPoint midJointPos,
                     AwPoint effectorPos,
                     AwPoint handlePos,
                     AwVector poleVector,
                     double twistValue,
                     AwQuaternion qStart,
                     AwQuaternion qMid)
        // This is method that actually computes the IK solution.
        //
        {
            // vector from startJoint to midJoint
            AwVector vector1 = midJointPos.sub(startJointPos);
            // vector from midJoint to effector
            AwVector vector2 = effectorPos.sub(midJointPos);
            // vector from startJoint to handle
            AwVector vectorH = handlePos.sub(startJointPos);
            // vector from startJoint to effector
            AwVector vectorE = effectorPos.sub(startJointPos);

            // lengths of those vectors
            double length1 = vector1.length();
            double length2 = vector2.length();
            double lengthH = vectorH.length();

            double   d       = vector1.mul(vectorE) / vectorE.mul(vectorE);
            AwVector vectorO = vector1.sub(vectorE.mul(d));

            //////////////////////////////////////////////////////////////////
            // calculate q12 which solves for the midJoint rotation
            //////////////////////////////////////////////////////////////////
            // angle between vector1 and vector2
            double vectorAngle12 = vector1.angle(vector2);

            // vector orthogonal to vector1 and 2
            AwVector vectorCross12  = vector1.crossProduct(vector2);
            double   lengthHsquared = lengthH * lengthH;

            // angle for arm extension
            double cos_theta =
                (lengthHsquared - length1 * length1 - length2 * length2)
                / (2 * length1 * length2);

            if (cos_theta > 1)
            {
                cos_theta = 1;
            }
            else if (cos_theta < -1)
            {
                cos_theta = -1;
            }

            double theta = Math.Acos(cos_theta);

            AwQuaternion q12 = new AwQuaternion(theta - vectorAngle12, vectorCross12);

            //////////////////////////////////////////////////////////////////
            // calculate qEH which solves for effector rotating onto the handle
            //////////////////////////////////////////////////////////////////
            // vector2 with quaternion q12 applied
            vector2 = vector2.rotateBy(q12);
            // vectorE with quaternion q12 applied
            vectorE = vector1.add(vector2);
            // quaternion for rotating the effector onto the handle
            AwQuaternion qEH = new AwQuaternion(vectorE, vectorH);

            // calculate qNP which solves for the rotate plane
            //////////////////////////////////////////////////////////////////
            // vector1 with quaternion qEH applied
            vector1 = vector1.rotateBy(qEH);
            if (vector1.isParallel(vectorH, AwMath.kDoubleEpsilon))
            {
                // singular case, use orthogonal component instead
                vector1 = vectorO.rotateBy(qEH);
            }

            AwQuaternion qNP = new AwQuaternion();

            if (!poleVector.isParallel(vectorH, AwMath.kDoubleEpsilon) && (lengthHsquared != 0))
            {
                double   temp    = poleVector.mul(vectorH) / lengthHsquared;
                AwVector vectorN = vector1.sub(vectorH.mul(temp));

                AwVector vectorP = poleVector.sub(vectorH.mul(vector1.mul(vectorH) / lengthHsquared));

                double dotNP = (vectorN.mul(vectorP)) / (vectorN.length() * vectorP.length());

                if (Math.Abs(dotNP + 1.0) < kEpsilon)
                {
                    // singular case, rotate halfway around vectorH
                    AwQuaternion qNP1 = new AwQuaternion(AwMath.kPi, vectorH);
                    qNP = qNP1;
                }
                else
                {
                    AwQuaternion qNP2 = new AwQuaternion(vectorN, vectorP);
                    qNP = qNP2;
                }
            }

            //////////////////////////////////////////////////////////////////
            // calculate qTwist which adds the twist
            //////////////////////////////////////////////////////////////////
            AwQuaternion qTwist = new AwQuaternion(twistValue, vectorH);

            // quaternion for the mid joint
            qMid = q12;
            // concatenate the quaternions for the start joint
            AwQuaternion qTemp = qEH.mul(qNP);

            qStart = qTemp.mul(qTwist);
        }