예제 #1
0
        public AwVector normal()
        {
            AwVector tmp = new AwVector(this);

            tmp.normalize();
            return(tmp);
        }
예제 #2
0
        public AwQuaternion setAxisAngle(AwVector axis, double theta)
        {
            double sumOfSquares =
                (double)axis.x * axis.x +
                (double)axis.y * axis.y +
                (double)axis.z * axis.z;

            if (sumOfSquares <= AwMath.kDoubleEpsilon)
            {
                w = 1.0;
                x = 0.0;
                y = 0.0;
                z = 0.0;
            }
            else
            {
                theta *= 0.5;
                w      = Math.Cos(theta);
                double commonFactor = Math.Sin(theta);
                if (!AwMath.equivalent(sumOfSquares, 1.0))
                {
                    commonFactor /= Math.Sqrt(sumOfSquares);
                }

                x = commonFactor * (double)axis.x;
                y = commonFactor * (double)axis.y;
                z = commonFactor * (double)axis.z;
            }

            return(this);
        }
예제 #3
0
        public bool getAxisAngle(AwVector axis, ref double theta)
        {
            bool   result;
            double inverseOfSinThetaByTwo, thetaExtended;

            if (AwMath.equivalent(w, (double)1.0))
            {
                theta = 0.0;
                if (axis.length() < AwMath.kDoubleEpsilon)
                {
                    axis.set(0.0, 0.0, 1.0);
                }
                result = false;
            }
            else
            {
                thetaExtended = Math.Acos(AwMath.clamp(w, -1.0, 1.0));
                theta         = thetaExtended * 2.0;

                inverseOfSinThetaByTwo = 1.0 / Math.Sin(thetaExtended);
                axis.x = x * inverseOfSinThetaByTwo;
                axis.y = y * inverseOfSinThetaByTwo;
                axis.z = z * inverseOfSinThetaByTwo;

                result = true;
            }

            return(result);
        }
예제 #4
0
 public AwQuaternion(double angle, AwVector axis)
 {
     w = 1.0;
     x = 0.0;
     y = 0.0;
     z = 0.0;
     setAxisAngle(axis, angle);
 }
예제 #5
0
 public AwQuaternion(double angle, AwVector axis)
 {
     w = 1.0;
     x = 0.0;
     y = 0.0;
     z = 0.0;
     setAxisAngle(axis, angle);
 }
예제 #6
0
 public AwVector add(AwVector v)
 {
     AwVector tmp = new AwVector(this);
     tmp.x = tmp.x + v.x;
     tmp.y = tmp.y + v.y;
     tmp.z = tmp.z + v.z;
     return tmp;
 }
예제 #7
0
        public AwVector mulMatrix(AwMatrix matrix)
        {
            AwVector tmp = new AwVector();

            tmp.x = x * matrix.getIndex(0, 0) + y * matrix.getIndex(1, 0) + z * matrix.getIndex(2, 0);
            tmp.y = x * matrix.getIndex(0, 1) + y * matrix.getIndex(1, 1) + z * matrix.getIndex(2, 1);
            tmp.z = x * matrix.getIndex(0, 2) + y * matrix.getIndex(1, 2) + z * matrix.getIndex(2, 2);
            return(tmp);
        }
예제 #8
0
        public AwVector sub(AwVector v)
        {
            AwVector tmp = new AwVector(this);

            tmp.x = tmp.x - v.x;
            tmp.y = tmp.y - v.y;
            tmp.z = tmp.z - v.z;
            return(tmp);
        }
예제 #9
0
        public AwVector add(AwVector v)
        {
            AwVector tmp = new AwVector(this);

            tmp.x = tmp.x + v.x;
            tmp.y = tmp.y + v.y;
            tmp.z = tmp.z + v.z;
            return(tmp);
        }
예제 #10
0
        public AwVector mul(double s)
        {
            AwVector tmp = new AwVector(this);

            tmp.x = tmp.x * s;
            tmp.y = tmp.y * s;
            tmp.z = tmp.z * s;
            return(tmp);
        }
예제 #11
0
        public bool isParallel(AwVector otherVector, double tolerance)
        {
            AwVector v1, v2;

            v1 = normal();
            v2 = otherVector.normal();
            double dotPrd = v1.dotProduct(v2);

            return(AwMath.equivalent(Math.Abs(dotPrd), (double)1.0, tolerance));
        }
예제 #12
0
        public AwVector rotateBy(AwQuaternion q)
        {
            double   rw = -q.x * x - q.y * y - q.z * z;
            double   rx = q.w * x + q.y * z - q.z * y;
            double   ry = q.w * y + q.z * x - q.x * z;
            double   rz = q.w * z + q.x * y - q.y * x;
            AwVector v  = new AwVector(-rw * q.x + rx * q.w - ry * q.z + rz * q.y,
                                       -rw * q.y + ry * q.w - rz * q.x + rx * q.z,
                                       -rw * q.z + rz * q.w - rx * q.y + ry * q.x);

            return(v);
        }
예제 #13
0
 public double angle(AwVector vec)
 {
     double cosine = normal().dotProduct(vec.normal());
     double angle;
     if (cosine >= 1.0)
         angle = 0.0;
     else if (cosine <= -1.0)
         angle = AwMath.kPi;
     else
         angle = Math.Acos(cosine);
     return angle;
 }
예제 #14
0
        AwVector poleVectorFromHandle(MDagPath handlePath)
        {
            MFnIkHandle handleFn = new MFnIkHandle(handlePath);
            MPlug       pvxPlug = handleFn.findPlug("pvx");
            MPlug       pvyPlug = handleFn.findPlug("pvy");
            MPlug       pvzPlug = handleFn.findPlug("pvz");
            double      pvxValue, pvyValue, pvzValue;

            pvxValue = pvyValue = pvzValue = 0;
            pvxPlug.getValue(pvxValue);
            pvyPlug.getValue(pvyValue);
            pvzPlug.getValue(pvzValue);
            AwVector poleVector = new AwVector(pvxValue, pvyValue, pvzValue);

            return(poleVector);
        }
예제 #15
0
        public double angle(AwVector vec)
        {
            double cosine = normal().dotProduct(vec.normal());
            double angle;

            if (cosine >= 1.0)
            {
                angle = 0.0;
            }
            else if (cosine <= -1.0)
            {
                angle = AwMath.kPi;
            }
            else
            {
                angle = Math.Acos(cosine);
            }
            return(angle);
        }
예제 #16
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);
            }
        }
예제 #17
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);
            }
        }
예제 #18
0
 public double dotProduct(AwVector v)
 {
     return(x * v.x + y * v.y + z * v.z);
 }
예제 #19
0
 public AwVector(AwVector v)
 {
     x = v.x;
     y = v.y;
     z = v.z;
 }
예제 #20
0
 public AwVector(AwVector v)
 {
     x = v.x;
     y = v.y;
     z = v.z;
 }
예제 #21
0
        public bool getAxisAngle(AwVector axis, ref double theta)
        {
            bool result;
	        double inverseOfSinThetaByTwo, thetaExtended;
	
	        if (AwMath.equivalent(w, (double) 1.0)) 
            {
		        theta = 0.0;
		        if (axis.length() < AwMath.kDoubleEpsilon) 
                {
			        axis.set(0.0,0.0,1.0);
		        }
		        result = false;
	        }
	        else 
            {
		        thetaExtended = Math.Acos(AwMath.clamp(w,-1.0,1.0));
		        theta = thetaExtended * 2.0;
		
		        inverseOfSinThetaByTwo = 1.0 / Math.Sin(thetaExtended);
		        axis.x = x * inverseOfSinThetaByTwo;
		        axis.y = y * inverseOfSinThetaByTwo;
		        axis.z = z * inverseOfSinThetaByTwo;

		        result = true;
	        }

	        return result;
        }
예제 #22
0
 public AwVector normal()
 {
     AwVector tmp = new AwVector(this);
     tmp.normalize();
     return tmp;
 }
예제 #23
0
        // This is method that actually computes the IK solution.
        //
        void solveIK( AwPoint startJointPos,
            AwPoint midJointPos,
            AwPoint effectorPos,
            AwPoint handlePos,
            AwVector poleVector,
            double twistValue,
            AwQuaternion qStart,
            AwQuaternion qMid)
        {
            // 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);
        }
예제 #24
0
 AwVector poleVectorFromHandle( MDagPath handlePath)
 {
     MFnIkHandle handleFn = new MFnIkHandle(handlePath);
     MPlug pvxPlug = handleFn.findPlug("pvx");
     MPlug pvyPlug = handleFn.findPlug("pvy");
     MPlug pvzPlug = handleFn.findPlug("pvz");
     double pvxValue, pvyValue, pvzValue;
     pvxValue=pvyValue=pvzValue=0;
     pvxPlug.getValue(pvxValue);
     pvyPlug.getValue(pvyValue);
     pvzPlug.getValue(pvzValue);
     AwVector poleVector = new AwVector(pvxValue, pvyValue, pvzValue);
     return poleVector;
 }
예제 #25
0
 public AwVector crossProduct( AwVector r)
 {
     return new AwVector(y*r.z - z*r.y, z*r.x - x*r.z, x*r.y - y*r.x);
 }
예제 #26
0
 public AwVector rotateBy( AwQuaternion q)
 {
     double rw = -q.x * x - q.y * y - q.z * z;
     double rx = q.w * x + q.y * z - q.z * y;
     double ry = q.w * y + q.z * x - q.x * z;
     double rz = q.w * z + q.x * y - q.y * x;
     AwVector v = new AwVector(-rw * q.x + rx * q.w - ry * q.z + rz * q.y,
                     -rw * q.y + ry * q.w - rz * q.x + rx * q.z,
                     -rw * q.z + rz * q.w - rx * q.y + ry * q.x);
     return v;
 }
예제 #27
0
 public AwVector crossProduct(AwVector r)
 {
     return(new AwVector(y * r.z - z * r.y, z * r.x - x * r.z, x * r.y - y * r.x));
 }
예제 #28
0
 public double dotProduct(AwVector v)
 {
     return x * v.x + y * v.y + z * v.z;
 }
예제 #29
0
 public AwVector mulMatrix(AwMatrix matrix)
 {
     AwVector tmp = new AwVector();
     tmp.x = x * matrix.getIndex(0, 0) + y * matrix.getIndex(1, 0) + z * matrix.getIndex(2, 0);
     tmp.y = x * matrix.getIndex(0, 1) + y * matrix.getIndex(1, 1) + z * matrix.getIndex(2, 1);
     tmp.z = x * matrix.getIndex(0, 2) + y * matrix.getIndex(1, 2) + z * matrix.getIndex(2, 2);
     return tmp;
 }
예제 #30
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);
        }
예제 #31
0
 public AwVector mul(double s)
 {
     AwVector tmp = new AwVector(this);
     tmp.x = tmp.x * s;
     tmp.y = tmp.y * s;
     tmp.z = tmp.z * s;
     return tmp;
 }
예제 #32
0
        public override void doSolve()
        {
            MIkHandleGroup handle_group = handleGroup;

            if (handle_group == null)
            {
                throw new InvalidOperationException("Invalid handle group");
            }

            MObject     handle     = handle_group.handle(0);
            MDagPath    handlePath = MDagPath.getAPathTo(handle);
            MFnIkHandle handleFn   = new MFnIkHandle(handlePath);

            //Effector
            //
            MDagPath effectorPath = new MDagPath();

            handleFn.getEffector(effectorPath);
            MFnIkEffector effectorFn = new MFnIkEffector(effectorPath);

            effectorPath.pop();
            MFnIkJoint midJoinFn = new MFnIkJoint(effectorPath);

            // Start Joint
            //
            MDagPath startJointPath = new MDagPath();

            handleFn.getStartJoint(startJointPath);
            MFnIkJoint startJointFn = new MFnIkJoint(startJointPath);

            // Preferred angles
            //
            double [] startJointPrefAngle = new double[3];
            double[]  midJointPrefAngle   = new double[3];

            startJointFn.getPreferedAngle(startJointPrefAngle);
            midJoinFn.getPreferedAngle(midJointPrefAngle);

            // Set to preferred angles
            //
            startJointFn.setRotation(startJointPrefAngle, startJointFn.rotationOrder);

            midJoinFn.setRotation(midJointPrefAngle, midJoinFn.rotationOrder);

            MPoint  handlePos   = handleFn.rotatePivot(MSpace.Space.kWorld);
            AwPoint awHandlePos = new AwPoint(handlePos.x, handlePos.y, handlePos.z, handlePos.w);

            MPoint  effectorPos   = effectorFn.rotatePivot(MSpace.Space.kWorld);
            AwPoint awEffectorPos = new AwPoint(effectorPos.x, effectorPos.y, effectorPos.z, effectorPos.w);

            MPoint  midJoinPos   = midJoinFn.rotatePivot(MSpace.Space.kWorld);
            AwPoint awMidJoinPos = new AwPoint(midJoinPos.x, midJoinPos.y, midJoinPos.z, midJoinPos.w);

            MPoint  startJointPos   = startJointFn.rotatePivot(MSpace.Space.kWorld);
            AwPoint awStartJointPos = new AwPoint(startJointPos.x, startJointPos.y, startJointPos.z, startJointPos.w);

            AwVector poleVector = poleVectorFromHandle(handlePath);
            MMatrix  m          = handlePath.exclusiveMatrix;
            AwMatrix awM        = new AwMatrix();

            awM.setMatrix(m);
            poleVector = poleVector.mulMatrix(awM);
            double twistValue = twistFromHandle(handlePath);

            AwQuaternion qStart = new AwQuaternion();
            AwQuaternion qMid   = new AwQuaternion();

            solveIK(awStartJointPos, awMidJoinPos, awEffectorPos, awHandlePos,
                    poleVector, twistValue, qStart, qMid);

            MQuaternion mid   = new MQuaternion(qMid.x, qMid.y, qMid.z, qMid.w);
            MQuaternion start = new MQuaternion(qStart.x, qStart.y, qStart.z, qStart.w);

            midJoinFn.rotateBy(mid, MSpace.Space.kWorld);
            startJointFn.rotateBy(start, MSpace.Space.kWorld);

            return;
        }
예제 #33
0
 public double mul(AwVector v)
 {
     return dotProduct(v);
 }
예제 #34
0
 public double mul(AwVector v)
 {
     return(dotProduct(v));
 }
예제 #35
0
 public bool isParallel( AwVector otherVector, double tolerance)
 {
     AwVector v1, v2;
     v1 = normal();
     v2 = otherVector.normal();
     double dotPrd = v1.dotProduct(v2);
     return (AwMath.equivalent( Math.Abs(dotPrd), (double) 1.0, tolerance));
 }
예제 #36
0
        public AwQuaternion setAxisAngle(AwVector axis, double theta)
        {
            double sumOfSquares = 
		        (double) axis.x * axis.x +
		        (double) axis.y * axis.y +
		        (double) axis.z * axis.z;
		
            if (sumOfSquares <= AwMath.kDoubleEpsilon) 
            {
		        w = 1.0;
                x = 0.0;
                y = 0.0;
                z = 0.0;
            } 
            else 
            {
		        theta *= 0.5;
		        w = Math.Cos(theta);
		        double commonFactor = Math.Sin(theta);
		        if (!AwMath.equivalent(sumOfSquares, 1.0)) 
			        commonFactor /= Math.Sqrt(sumOfSquares);
		        
                x = commonFactor * (double) axis.x;
		        y = commonFactor * (double) axis.y;
		        z = commonFactor * (double) axis.z;
	        }
	        
            return this;
        }
예제 #37
0
 public AwVector sub(AwVector v)
 {
     AwVector tmp = new AwVector(this);
     tmp.x = tmp.x - v.x;
     tmp.y = tmp.y - v.y;
     tmp.z = tmp.z - v.z;
     return tmp;
 }