Math3d.cs

            bool lineCCrossing = AreLineSegmentsCrossing(linePoint1, linePoint2, rectC, rectD);
            bool lineDCrossing = AreLineSegmentsCrossing(linePoint1, linePoint2, rectD, rectA);

            if (lineACrossing || lineBCrossing || lineCCrossing || lineDCrossing)
            {

                return true;
            }

            else
            {

                return false;
            }
        }

        else
        {

            return true;
        }
    }

    //Returns true if "point" is in a rectangle mad up of RectA to RectD. The line point is assumed to be on the same 
    //plane as the rectangle. If the point is not on the plane, use ProjectPointOnPlane() first.
    public static bool IsPointInRectangle(Vector3 point, Vector3 rectA, Vector3 rectC, Vector3 rectB, Vector3 rectD)
    {

        Vector3 vector;
        Vector3 linePoint;

        //get the center of the rectangle
        vector = rectC - rectA;
        float size = -(vector.magnitude / 2f);
        vector = AddVectorLength(vector, size);
        Vector3 middle = rectA + vector;

        Vector3 xVector = rectB - rectA;
        float width = xVector.magnitude / 2f;

        Vector3 yVector = rectD - rectA;
        float height = yVector.magnitude / 2f;

        linePoint = ProjectPointOnLine(middle, xVector.normalized, point);
        vector = linePoint - point;
        float yDistance = vector.magnitude;

        linePoint = ProjectPointOnLine(middle, yVector.normalized, point);
        vector = linePoint - point;
        float xDistance = vector.magnitude;

        if ((xDistance <= width) && (yDistance <= height))
        {

            return true;
        }

        else
        {

            return false;
        }
    }

    //Returns true if line segment made up of pointA1 and pointA2 is crossing line segment made up of
    //pointB1 and pointB2. The two lines are assumed to be in the same plane.
    public static bool AreLineSegmentsCrossing(Vector3 pointA1, Vector3 pointA2, Vector3 pointB1, Vector3 pointB2)
    {

        Vector3 closestPointA;
        Vector3 closestPointB;
        int sideA;
        int sideB;

        Vector3 lineVecA = pointA2 - pointA1;
        Vector3 lineVecB = pointB2 - pointB1;

        bool valid = ClosestPointsOnTwoLines(out closestPointA, out closestPointB, pointA1, lineVecA.normalized, pointB1, lineVecB.normalized);

        //lines are not parallel
        if (valid)
        {

            sideA = PointOnWhichSideOfLineSegment(pointA1, pointA2, closestPointA);
            sideB = PointOnWhichSideOfLineSegment(pointB1, pointB2, closestPointB);

            if ((sideA == 0) && (sideB == 0))
            {

                return true;
            }

            else
            {

                return false;
            }
        }

        //lines are parallel
        else
        {

            return false;
        }
    }

    //This function calculates the acceleration vector in meter/second^2.
    //Input: position. If the output is used for motion simulation, the input transform
    //has to be located at the seat base, not at the vehicle CG. Attach an empty GameObject
    //at the correct location and use that as the input for this function.
    //Gravity is not taken into account but this can be added to the output if needed.
    //A low number of samples can give a jittery result due to rounding errors.
    //If more samples are used, the output is more smooth but has a higher latency.
    public static bool LinearAcceleration(out Vector3 vector, Vector3 position, int samples)
    {

        Vector3 averageSpeedChange = Vector3.zero;
        vector = Vector3.zero;
        Vector3 deltaDistance;
        float deltaTime;
        Vector3 speedA;
        Vector3 speedB;

        //Clamp sample amount. In order to calculate acceleration we need at least 2 changes
        //in speed, so we need at least 3 position samples.
        if (samples < 3)
        {

            samples = 3;
        }

        //Initialize
        if (positionRegister == null)
        {

            positionRegister = new Vector3[samples];
            posTimeRegister = new float[samples];
        }

        //Fill the position and time sample array and shift the location in the array to the left
        //each time a new sample is taken. This way index 0 will always hold the oldest sample and the
        //highest index will always hold the newest sample. 
        for (int i = 0; i < positionRegister.Length - 1; i++)
        {

            positionRegister[i] = positionRegister[i + 1];
            posTimeRegister[i] = posTimeRegister[i + 1];
        }
        positionRegister[positionRegister.Length - 1] = position;
        posTimeRegister[posTimeRegister.Length - 1] = Time.time;

        positionSamplesTaken++;

        //The output acceleration can only be calculated if enough samples are taken.
        if (positionSamplesTaken >= samples)
        {

            //Calculate average speed change.
            for (int i = 0; i < positionRegister.Length - 2; i++)
            {

                deltaDistance = positionRegister[i + 1] - positionRegister[i];
                deltaTime = posTimeRegister[i + 1] - posTimeRegister[i];

                //If deltaTime is 0, the output is invalid.
                if (deltaTime == 0)
                {

                    return false;
                }

                speedA = deltaDistance / deltaTime;
                deltaDistance = positionRegister[i + 2] - positionRegister[i + 1];
                deltaTime = posTimeRegister[i + 2] - posTimeRegister[i + 1];

                if (deltaTime == 0)
                {

                    return false;
                }

                speedB = deltaDistance / deltaTime;

                //This is the accumulated speed change at this stage, not the average yet.
                averageSpeedChange += speedB - speedA;
            }

            //Now this is the average speed change.
            averageSpeedChange /= positionRegister.Length - 2;

            //Get the total time difference.
            float deltaTimeTotal = posTimeRegister[posTimeRegister.Length - 1] - posTimeRegister[0];

            //Now calculate the acceleration, which is an average over the amount of samples taken.
            vector = averageSpeedChange / deltaTimeTotal;

            return true;
        }

        else
        {

            return false;
        }
    }


    /*
	//This function calculates angular acceleration in object space as deg/second^2, encoded as a vector. 
	//For example, if the output vector is 0,0,-5, the angular acceleration is 5 deg/second^2 around the object Z axis, to the left. 
	//Input: rotation (quaternion). If the output is used for motion simulation, the input transform
	//has to be located at the seat base, not at the vehicle CG. Attach an empty GameObject
	//at the correct location and use that as the input for this function.
	//A low number of samples can give a jittery result due to rounding errors.
	//If more samples are used, the output is more smooth but has a higher latency.
	//Note: the result is only accurate if the rotational difference between two samples is less than 180 degrees.
	//Note: a suitable way to visualize the result is:
	Vector3 dir;
	float scale = 2f;	
	dir = new Vector3(vector.x, 0, 0);
	dir = Math3d.SetVectorLength(dir, dir.magnitude * scale);
	dir = gameObject.transform.TransformDirection(dir);
	Debug.DrawRay(gameObject.transform.position, dir, Color.red);	
	dir = new Vector3(0, vector.y, 0);
	dir = Math3d.SetVectorLength(dir, dir.magnitude * scale);
	dir = gameObject.transform.TransformDirection(dir);
	Debug.DrawRay(gameObject.transform.position, dir, Color.green);	
	dir = new Vector3(0, 0, vector.z);
	dir = Math3d.SetVectorLength(dir, dir.magnitude * scale);
	dir = gameObject.transform.TransformDirection(dir);
	Debug.DrawRay(gameObject.transform.position, dir, Color.blue);	*/
    public static bool AngularAcceleration(out Vector3 vector, Quaternion rotation, int samples)
    {

        Vector3 averageSpeedChange = Vector3.zero;
        vector = Vector3.zero;
        Quaternion deltaRotation;
        float deltaTime;
        Vector3 speedA;
        Vector3 speedB;

        //Clamp sample amount. In order to calculate acceleration we need at least 2 changes
        //in speed, so we need at least 3 rotation samples.
        if (samples < 3)
        {

            samples = 3;
        }

        //Initialize
        if (rotationRegister == null)
        {

            rotationRegister = new Quaternion[samples];
            rotTimeRegister = new float[samples];
        }

        //Fill the rotation and time sample array and shift the location in the array to the left
        //each time a new sample is taken. This way index 0 will always hold the oldest sample and the
        //highest index will always hold the newest sample. 
        for (int i = 0; i < rotationRegister.Length - 1; i++)
        {

            rotationRegister[i] = rotationRegister[i + 1];
            rotTimeRegister[i] = rotTimeRegister[i + 1];
        }
        rotationRegister[rotationRegister.Length - 1] = rotation;
        rotTimeRegister[rotTimeRegister.Length - 1] = Time.time;

        rotationSamplesTaken++;

        //The output acceleration can only be calculated if enough samples are taken.
        if (rotationSamplesTaken >= samples)
        {

            //Calculate average speed change.
            for (int i = 0; i < rotationRegister.Length - 2; i++)
            {

                deltaRotation = SubtractRotation(rotationRegister[i + 1], rotationRegister[i]);
                deltaTime = rotTimeRegister[i + 1] - rotTimeRegister[i];

                //If deltaTime is 0, the output is invalid.
                if (deltaTime == 0)
                {

                    return false;
                }

                speedA = RotDiffToSpeedVec(deltaRotation, deltaTime);
                deltaRotation = SubtractRotation(rotationRegister[i + 2], rotationRegister[i + 1]);
                deltaTime = rotTimeRegister[i + 2] - rotTimeRegister[i + 1];

                if (deltaTime == 0)
                {

                    return false;
                }

                speedB = RotDiffToSpeedVec(deltaRotation, deltaTime);

                //This is the accumulated speed change at this stage, not the average yet.
                averageSpeedChange += speedB - speedA;
            }

            //Now this is the average speed change.
            averageSpeedChange /= rotationRegister.Length - 2;

            //Get the total time difference.
            float deltaTimeTotal = rotTimeRegister[rotTimeRegister.Length - 1] - rotTimeRegister[0];

            //Now calculate the acceleration, which is an average over the amount of samples taken.
            vector = averageSpeedChange / deltaTimeTotal;

            return true;
        }

        else
        {

            return false;
        }
    }

    //Get y from a linear function, with x as an input. The linear function goes through points
    //0,0 on the left ,and Qxy on the right.
    public static float LinearFunction2DBasic(float x, float Qx, float Qy)
    {

        float y = x * (Qy / Qx);

        return y;
    }

    //Get y from a linear function, with x as an input. The linear function goes through points
    //Pxy on the left ,and Qxy on the right.
    public static float LinearFunction2DFull(float x, float Px, float Py, float Qx, float Qy)
    {

        float y = 0f;

        float A = Qy - Py;
        float B = Qx - Px;
        float C = A / B;

        y = Py + (C * (x - Px));

        return y;
    }

    //Convert a rotation difference to a speed vector.
    //For internal use only.
    private static Vector3 RotDiffToSpeedVec(Quaternion rotation, float deltaTime)
    {

        float x;
        float y;
        float z;

        if (rotation.eulerAngles.x <= 180.0f)
        {

            x = rotation.eulerAngles.x;
        }

        else
        {

            x = rotation.eulerAngles.x - 360.0f;
        }

        if (rotation.eulerAngles.y <= 180.0f)
        {

            y = rotation.eulerAngles.y;
        }

        else
        {

            y = rotation.eulerAngles.y - 360.0f;
        }

        if (rotation.eulerAngles.z <= 180.0f)
        {

            z = rotation.eulerAngles.z;
        }

        else
        {

            z = rotation.eulerAngles.z - 360.0f;
        }

        return new Vector3(x / deltaTime, y / deltaTime, z / deltaTime);
    }
}