Example #1
0
    void Start()
    {
        // float value = quaternionTest.Modulus();

        // MULTIPLICATION

        /*
         * m = multiply.Multiply(q1, q2);
         * Debug.Log(m.x + ":" + m.y + ":" + m.z + ":" + m.w);
         */

        // Axis to Quaternion

        //m.Axis2Quad(120, new Vector3(-0.5774f, 0.5774f, 0.5774f));
        //Debug.Log(m.x + ":" + m.y + ":" + m.z + ":" + m.w);

        // Quaternion to Axis
        q2a = q2a.Quad2Axis(m);
        //Debug.Log(q2a.x + ":" + q2a.y + ":" + q2a.z + ":" + q2a.w);

        Quaternion angle1 = new Quaternion(3.5f, -10, 5.7f, 1.7f);
        Quaternion angle2 = new Quaternion(9, -5.3f, +4, -6.6f);

        Debug.Log("Ours: " + MyQuat.Angle(q1, q2));
        Debug.Log("Unity: " + Quaternion.Angle(angle1, angle2));
    }
Example #2
0
    void Update()
    {
        // Copy the joints positions to work with
        //TODO
        copy[0] = new MyVector3(joints[0].position);

        //copy[0] = joints[0].position;

        for (int i = 0; i < joints.Length - 1; i++)
        {
            copy[i + 1]  = new MyVector3(joints[i + 1].position);
            distances[i] = (copy[i + 1] - copy[i]).Module();

            //copy[i + 1] = joints[i + 1].position;
            //distances[i] = (copy[i + 1] - copy[i]).magnitude;
        }
        // CALCULATE ALSO THE DISTANCE BETWEEN JOINTS

        //done = TODO
        done = (copy[copy.Length - 1] - new MyVector3(target.position)).Module() < tresholdCondition;

        //done = (copy[copy.Length - 1] - target.position).magnitude < tresholdCondition;

        if (!done)
        {
            float targetRootDist = (copy[0] - new MyVector3(target.position)).Module();

            //float targetRootDist = Vector3.Distance(copy[0], target.position);

            // Update joint positions
            if (targetRootDist > distances.Sum())
            {
                // The target is unreachable
                for (int i = 0; i < copy.Length - 1; i++)
                {
                    float r = (new MyVector3(target.position) - copy[i]).Module();

                    //float r = (target.position - copy[i]).magnitude;
                    float lambda = distances[i] / r;

                    copy[i + 1] = (1 - lambda) * copy[i] + (lambda * new MyVector3(target.position));

                    //copy[i + 1] = (1 - lambda) * copy[i] + (lambda * target.position);
                }
            }
            else
            {
                MyVector3 b = copy[0];

                //Vector3 b = copy[0];

                // The target is reachable
                //while (TODO)

                float difference = (copy[copy.Length - 1] - new MyVector3(target.position)).Module();

                //float difference = (copy[copy.Length - 1] - target.position).magnitude;

                while (difference > tresholdCondition) // treshold = tolerance
                {
                    // numIterations++;

                    // STAGE 1: FORWARD REACHING
                    //TODO
                    copy[copy.Length - 1] = new MyVector3(target.position);

                    //copy[copy.Length - 1] = target.position;

                    for (int i = copy.Length - 2; i > 0; i--)
                    {
                        float r = (copy[i + 1] - copy[i]).Module();

                        //float r = (copy[i + 1] - copy[i]).magnitude;
                        float lambda = distances[i] / r;

                        copy[i] = (1 - lambda) * copy[i + 1] + lambda * copy[i];
                    }

                    // STAGE 2: BACKWARD REACHING
                    //TODO
                    copy[0] = b;

                    for (int i = 0; i < copy.Length - 1; i++)
                    {
                        float r = (copy[i + 1] - copy[i]).Module();

                        //float r = (copy[i + 1] - copy[i]).magnitude;
                        float lambda = distances[i] / r;

                        copy[i + 1] = (1 - lambda) * copy[i] + lambda * copy[i + 1];
                    }

                    difference = (copy[copy.Length - 1] - new MyVector3(target.position)).Module();

                    //difference = (copy[copy.Length - 1] - target.position).magnitude;
                }
            }

            // Update original joint rotations
            for (int i = 0; i <= joints.Length - 2; i++)
            {
                // float originalAngle = joints[i].rotation.w;

                MyQuat parentRotation = new MyQuat(joints[i + 1].rotation);
                MyQuat childRotation  = new MyQuat(joints[i].rotation);

                //TODO
                // Rotation
                MyVector3 vectorA = new MyVector3(joints[i + 1].position) - new MyVector3(joints[i].position);
                MyVector3 vectorB = copy[i + 1] - copy[i];

                //Vector3 vectorA = joints[i + 1].position - joints[i].position;
                //Vector3 vectorB = copy[i + 1] - copy[i];

                // float angle = Mathf.Acos(Vector3.Dot(vectorA.normalized, vectorB.normalized)) * Mathf.Rad2Deg;
                float cosA = (MyVector3.Dot(vectorA.Normalize(), vectorB.Normalize()));
                float sinA = MyVector3.Cross(vectorA.Normalize(), vectorB.Normalize()).Module();

                //float cosA = (Vector3.Dot(vectorA.normalized, vectorB.normalized));
                //float sinA = Vector3.Cross(vectorA.normalized, vectorB.normalized).magnitude;

                // Atan = Cos | Atan2 = denominador y...
                float angle = Mathf.Atan2(sinA, cosA) * Mathf.Rad2Deg;

                MyVector3 axis = MyVector3.Cross(vectorA, vectorB).Normalize();

                //Vector3 axis = Vector3.Cross(vectorA, vectorB).normalized;

                // joints[i].rotation = Quaternion.AngleAxis(angle, axis) * joints[i].rotation;

                joints[i].rotation = MyQuat.Multiply(MyQuat.Axis2Quad(angle, axis), childRotation).ToUnityQuat();


                //joints[i].rotation = MyQuat.Multiply(MyQuat.Axis2Quad(angle, axis), childRotation).ToUnityQuat();

                childRotation = new MyQuat(joints[i].rotation);

                float angleTest = MyQuat.Angle(parentRotation, childRotation);

                if (Mathf.Abs(angleTest) > maxAngleRotation)
                {
                    joints[i + 1].rotation = joints[i].rotation;
                }


                joints[i + 1].position = new Vector3(copy[i + 1].x, copy[i + 1].y, copy[i + 1].z);


                //joints[i + 1].position = copy[i + 1];
            }
        }
    }