static void rotateAroundAxis(DoubleTransform dtr, DVec3 localAxis, double angle)
    {
        var axis = (dtr.localRotation * localAxis).normalized;
        var rot  = DQuat.fromAxisAngle(axis, angle).normalized;

        dtr.localRotation = (rot * dtr.localRotation).normalized;
    }
Esempio n. 2
0
    public static DQuat slerp(DQuat a, DQuat b, double t)
    {
        double dotVal = dot(a, b);
        double angle  = Math.Acos(dotVal);

        return((a * Math.Sin(angle * (1.0 - t)) + b * Math.Sin(angle * t)) / Math.Sin(angle));
    }
Esempio n. 3
0
    public static DVec3 operator*(DQuat q, DVec3 v)
    {
        var   qv     = new DQuat(v.x, v.y, v.z, 0.0);
        DQuat result = q * qv * q.conjugate;

        return(new DVec3(result.x, result.y, result.z));
    }
Esempio n. 4
0
    //DEBUG
    void DrawRotation_Trail()
    {
        int          Length = (Quat.Count < TrailLength) ? Quat.Count : TrailLength;
        LineRenderer Quat_Trail_Renderer = Quat_Trail.GetComponent <LineRenderer>();

        Quat_Trail_Renderer.positionCount = Length - 1;

        for (int i = 0; i < Length - 1; i++)
        { //Debug
            Quaternion DQuat;
            Vector3    QuatAxis;
            float      QuatAngle;

            if (Quat.Count < 2)
            {
                DQuat = Quaternion.identity;
            }
            else
            {
                DQuat = Quaternion.Inverse(Quat[Quat.Count - 2 - i]) * Quat[Quat.Count - 1 - i];
            }

            DQuat.ToAngleAxis(out QuatAngle, out QuatAxis);
            Vector3 TQuatAxis = RefMatInv * (Pos[Pos.Count - 1 - i] + QuatAxis.normalized * QuatAngle * GizmoQuatScale);
            Quat_Trail_Renderer.SetPosition(i, TQuatAxis + RefPos);
        }

        Quat_Trail_Renderer.startWidth = 0.01f * TrailLinesWidth;
        Quat_Trail_Renderer.endWidth   = TrailLinesWidth;
    }
Esempio n. 5
0
 public static DQuat lerp(DQuat a, DQuat b, double t)
 {
     return(new DQuat(
                (b.x - a.x) * t + a.x,
                (b.y - a.y) * t + a.y,
                (b.z - a.z) * t + a.z,
                (b.w - a.w) * t + a.w
                ).normalized);
 }
Esempio n. 6
0
    public override bool Equals(object other)
    {
        if (!(other is DQuat))
        {
            return(false);
        }
        DQuat v = (DQuat)other;

        return(v.x.Equals(x) && v.y.Equals(y) && v.z.Equals(z) && v.w.Equals(w));
    }
 void setRotation(DQuat rotation)
 {
     localRotation = rotation * getParentRotation().conjugate;
 }
Esempio n. 8
0
 public static double dot(DQuat a, DQuat b)
 {
     return((a.x * b.x) + (a.y * b.y) + (a.z * b.z) + (a.w * b.w));
 }
Esempio n. 9
0
    void Update()
    {
        if (Record)
        {
            RecordData();
        }

        // Draw Vector
        if (Vector)
        {
            if (SpeedActive)
            {
                Speed_Vect.SetActive(true);
                DrawVector(Speed_Vect, Speed[Speed.Count - 1], GizmoSpeedScale);
            }
            else
            {
                Speed_Vect.SetActive(false);
            }

            if (AccActive)
            {
                Acc_Vect.SetActive(true);
                DrawVector(Acc_Vect, Acc[Acc.Count - 1], GizmoAccScale);
            }
            else
            {
                Acc_Vect.SetActive(false);
            }

            if (JerkActive)
            {
                Jerk_Vect.SetActive(true);
                DrawVector(Jerk_Vect, Jerk[Jerk.Count - 1], GizmoJerkScale);
            }
            else
            {
                Jerk_Vect.SetActive(false);
            }
            if (OrientationActive)
            {
                Vector3 QuatAxis;
                float   QuatAngle;

                Quaternion DQuat;
                if (Quat.Count < 2)
                {
                    DQuat = Quaternion.identity;
                }
                else
                {
                    DQuat = Quaternion.Inverse(Quat[Quat.Count - 2]) * Quat[Quat.Count - 1];
                }

                DQuat.ToAngleAxis(out QuatAngle, out QuatAxis);
                Quat_Vect.SetActive(true);
                DrawVector(Quat_Vect, QuatAxis * QuatAngle, GizmoQuatScale);
            }
            else
            {
                Quat_Vect.SetActive(false);
            }
        }
        else
        {
            Speed_Vect.SetActive(false);
            Acc_Vect.SetActive(false);
            Jerk_Vect.SetActive(false);
            Quat_Vect.SetActive(false);
        }


        //Draw Trail
        if (Trail)
        {
            //Draw Trails and vect
            if (PosActive)
            {
                Pos_Trail.SetActive(true);
                Draw_Trail(Pos, Pos_Trail, 1);
            }
            else
            {
                Pos_Trail.SetActive(false);
            }


            if (SpeedActive)
            {
                Speed_Ribbon.SetActive(true);
                Draw_Ribbon(Speed, Pos, Speed_Ribbon, GizmoSpeedScale);
            }
            else
            {
                Speed_Ribbon.SetActive(false);
            }

            if (AccActive)
            {
                Acc_Ribbon.SetActive(true);
                Draw_Ribbon(Acc, Pos, Acc_Ribbon, GizmoAccScale);
            }
            else
            {
                Acc_Ribbon.SetActive(false);
            }

            if (JerkActive)
            {
                Jerk_Ribbon.SetActive(true);
                Draw_Ribbon(Jerk, Pos, Jerk_Ribbon, GizmoJerkScale);
            }
            else
            {
                Jerk_Ribbon.SetActive(false);
            }


            if (OrientationActive)
            {
                Quat_Trail.SetActive(true);
            }
            else
            {
                Quat_Trail.SetActive(false);
            }
        }
        else
        {
            Pos_Trail.SetActive(false);
            Speed_Ribbon.SetActive(false);
            Acc_Ribbon.SetActive(false);
            Jerk_Ribbon.SetActive(false);
            Quat_Trail.SetActive(false);
        }
    }