Esempio n. 1
0
        public static FVec3 RotateTowards(FVec3 current, FVec3 target, Fix64 maxRadiansDelta, Fix64 maxMagnitudeDelta)
        {
            Fix64 len1 = current.Magnitude();
            Fix64 len2 = target.Magnitude();

            if (len1 > Fix64.Epsilon && len2 > Fix64.Epsilon)
            {
                FVec3 from  = current / len1;
                FVec3 to    = target / len2;
                Fix64 cosom = Dot(from, to);

                if (cosom > Fix64.One - Fix64.Epsilon)
                {
                    return(MoveTowards(current, target, maxMagnitudeDelta));
                }
                if (cosom < -Fix64.One + Fix64.Epsilon)
                {
                    FQuat q = FQuat.AngleAxis(maxRadiansDelta * Fix64.RAD_TO_DEG, OrthoNormalVector(from));
                    return(q * from * ClampedMove(len1, len2, maxMagnitudeDelta));
                }
                else
                {
                    Fix64 angle = Fix64.Acos(cosom);
                    FQuat q     = FQuat.AngleAxis(Fix64.Min(maxRadiansDelta, angle) * Fix64.RAD_TO_DEG,
                                                  Normalize(Cross(from, to)));
                    return(q * from * ClampedMove(len1, len2, maxMagnitudeDelta));
                }
            }

            return(MoveTowards(current, target, maxMagnitudeDelta));
        }
Esempio n. 2
0
        public static FVec3 SlerpUnclamped(FVec3 from, FVec3 to, Fix64 t)
        {
            Fix64 scale0, scale1;

            Fix64 len2 = to.Magnitude();
            Fix64 len1 = from.Magnitude();
            FVec3 v2   = to / len2;
            FVec3 v1   = from / len1;

            Fix64 len   = (len2 - len1) * t + len1;
            Fix64 cosom = Dot(v1, v2);

            if (cosom > ( Fix64 )(1 - 1e-6))
            {
                scale0 = Fix64.One - t;
                scale1 = t;
            }
            else if (cosom < ( Fix64 )(-1 + 1e-6))
            {
                FVec3 axis = OrthoNormalVector(from);
                FQuat q    = FQuat.AngleAxis(( Fix64 )180 * t, axis);
                FVec3 v    = q * from * len;
                return(v);
            }
            else
            {
                Fix64 omega = Fix64.Acos(cosom);
                Fix64 sinom = Fix64.Sin(omega);
                scale0 = Fix64.Sin((Fix64.One - t) * omega) / sinom;
                scale1 = Fix64.Sin(t * omega) / sinom;
            }

            v2 = (v2 * scale1 + v1 * scale0) * len;
            return(v2);
        }
Esempio n. 3
0
        public FVec3 RotateAround(Fix64 angle, FVec3 axis)
        {
            // rotate into world space
            FQuat quaternion = FQuat.AngleAxis(Fix64.Zero, axis);

            quaternion = quaternion.Conjugate();
            FVec3 worldSpaceVector = this.Transform(quaternion);

            // rotate back to vector space
            quaternion       = FQuat.AngleAxis(angle, axis);
            worldSpaceVector = worldSpaceVector.Transform(quaternion);
            return(worldSpaceVector);
        }
Esempio n. 4
0
        public FVec3 Transform(FQuat quaternion)
        {
            Fix64 x2  = quaternion.x + quaternion.x;
            Fix64 y2  = quaternion.y + quaternion.y;
            Fix64 z2  = quaternion.z + quaternion.z;
            Fix64 xx2 = quaternion.x * x2;
            Fix64 xy2 = quaternion.x * y2;
            Fix64 xz2 = quaternion.x * z2;
            Fix64 yy2 = quaternion.y * y2;
            Fix64 yz2 = quaternion.y * z2;
            Fix64 zz2 = quaternion.z * z2;
            Fix64 wx2 = quaternion.w * x2;
            Fix64 wy2 = quaternion.w * y2;
            Fix64 wz2 = quaternion.w * z2;

            return(new FVec3
                   (
                       this.x * (Fix64.One - yy2 - zz2) + this.y * (xy2 - wz2) + this.z * (xz2 + wy2),
                       this.x * (xy2 + wz2) + this.y * (Fix64.One - xx2 - zz2) + this.z * (yz2 - wx2),
                       this.x * (xz2 - wy2) + this.y * (yz2 + wx2) + this.z * (Fix64.One - xx2 - yy2)
                   ));
        }