CityDrivingSimulator.Unity/Unity/Assets/simulation_framework/MathEx.cs

using UnityEngine;
using System.Collections;
using System.Diagnostics.CodeAnalysis;
using System;

public static class MathEx {
    public delegate float Fx(float x);
    public delegate float Fxy(float x, float y);

    public static double RadSToRpm = (60.0 / (2.0 * Math.PI));
    public static double RpmToRadS = ((2.0 * Math.PI) / 60.0);

    public static double Torque(double Power, double angularVelocity)
    {
        return Power/angularVelocity;
    }

    public static Vector3 Project(this Ray aRay, Vector3 aPoint)
    {
        Vector3 V1 = aPoint - aRay.origin;
        Vector3 V2 = Vector3.Project(V1, aRay.direction);
        return aRay.origin + V2;
    }

    public static double Power(double Torque, double angularVelocity) {
        return Torque * angularVelocity;
    }

    public static double Clamp(double value, double min, double max)
    {
        return value < min ? min : (value > max ? max : value);
    }

    public static double Clamp01(double value) {
        return value < 0.0f ? 0.0f : (value > 1.0f ? 1.0f : value);
    }

    public static float FindExtremumAgument(Fx func, float xMin, float xMax, float xStep) {
        float prevY = func(xMin);
        float prevDerivative = 0;
        while(xMin < xMax) {
            float y = func(xMin);
            float derivative = y - prevY;
            if(derivative < 0.00001f) {
                derivative = 0;
            }

            if((prevDerivative > 0) && (derivative <= 0)) {
                break;
            }
            prevDerivative = derivative;
            prevY = y;
            xMin += xStep;
        }
        return xMin;
    }

    public static float FindExtremumValue(Fx func, float xMin, float xMax, float xStep) {
        float prevY = func(xMin);
        float prevDerivative = 0;
        while(xMin < xMax) {
            float y = func(xMin);
            float derivative = y - prevY;
            if(derivative < 0.00001f) {
                derivative = 0;
            }

            if((prevDerivative > 0) && (derivative <= 0)) {
                break;
            }
            prevDerivative = derivative;
            prevY = y;
            xMin += xStep;
        }
        return prevY;
    }
}