OpticsSimulation.Unity/Assets/Polynom.cs

using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using System.Linq;

public class Polynom : MonoBehaviour {

    public float[] Coeffs = new float[] { 0, 0, 0.01f, 0, 0.01f };

    public PlotSampler Sampler;
    public TraceRay PlotTraceRay;
    public int lonSamples = 256;
    public int latSamples = 50;

    void Start () {
		
	}
	
	void Update () {
		
	}

    float Sagitta(float r) {
        float y = 0;
        int i = 0;
        for (i = 4; i >= (int) 2; i--) {
            
            y = y * r + Coeffs[i];
          
        }
        y *= Mathf.Pow(r, i + 1);
        return y;
    }

    float Derivative(float r){
        float y = 0;
        int i = 0;
        for (i = 4; i >= 2; i--) {
            y = y * r + i * Coeffs[i];
        }
        y *= Mathf.Pow(r, i);
        return y;
    }

    public float Fx(float x){
        return Mathf.Pow(x, 4) * Coeffs[4] + Mathf.Pow(x, 2) * Coeffs[2];
    }

    Vector3 Normal(Vector3 point){
        float r = Mathf.Sqrt(point.x * point.x + point.z * point.z);
        if(r == 0) {
            return Vector3.up;
        } else {
            var d = Derivative(r);
            return new Vector3(point.x * d / r, -1, point.z * d / r).normalized * -1.0f;
        }
    }

    float RayPlaneDistance(Ray plane, Ray ray)
    {
        return (Vector3.Dot(plane.origin, plane.direction) - Vector3.Dot(plane.direction, ray.origin)) / (Vector3.Dot(ray.direction, plane.direction));
    }

    bool Intersect(Ray ray, float startX, float maxX, ref Vector3 point)
    {
        Ray p = new Ray();




        // initial intersection with z=0 plane
        {
             float s = ray.direction.y;

             if (s == 0)
                 return false;

             float a = -ray.origin.y / s;

             if (a < 0)
                 return false;

             p.origin = (ray.origin + ray.direction * a).normalized * startX;
         }


       // p.origin = new Vector3(startX, 0, 0);
      //  p.direction = Normal(p.origin).normalized;



        int n = 64;      // avoid infinite loop

        while (n-- != 0) {
            float new_sag = Sagitta(new Vector2(p.origin.x, p.origin.z).magnitude);
            float old_sag = p.origin.y;

            // project previous intersection point on curve
            p.origin = new Vector3(p.origin.x, new_sag, p.origin.z);


            // stop if close enough
            if (Mathf.Abs(old_sag - new_sag) < 1e-6)
                break;

            // get curve tangeante plane at intersection point
            p.direction = Normal(p.origin);

            // intersect again with new tangeante plane
            float a = RayPlaneDistance(p, ray);

          //  if (a < 0)
            //    return false;

            p.origin = ray.origin + ray.direction * a;
        }

        if(n <=0) {
            return false;
        }

        if(Vector3.Dot(-ray.direction, Normal(p.origin)) < 0.0f) {
            return false;
        }

        if((new Vector2(p.origin.x, p.origin.z)).sqrMagnitude > (maxX * maxX)) {
            return false;
        }

        point = p.origin;

        return true;
    }

    bool Intersect(Ray ray, ref Vector3 point)
    {
        var extremums = FindExtremums(4.0f);
        extremums.Add(4);
        List<float> startPoints = new List<float>();
        for (int i = 0; i < extremums.Count - 1; ++i) {
            startPoints.Add((extremums[i] + extremums[i + 1]) * 0.5f);
        }

        List<Vector3> points = new List<Vector3>();
      

       

        foreach (var e in startPoints) {
            if (Intersect(ray, e, 4.0f, ref point)) {
                points.Add(point);
            }
        }
        if(points.Count > 0) {
            point = points.OrderBy(v => (v - ray.origin).magnitude).First();
            return true;
        } else {
            return false;
        }
    }


    List<float> FindExtremums(float range)
    {
        List<float> result = new List<float>();
        List<float> points = new List<float>();
        for (int i = 0; i < 10; ++i) {
            points.Add(Derivative(i / 10.0f * range));
        }

        for (int i = 0; i < 10 - 1; ++i) {
            if(Mathf.Abs(points[i]) < 0.000001f) {
                result.Add(points[i]);
            } else if (Mathf.Abs(points[i + 1]) < 0.000001f) {
                result.Add(points[i + 1]);
                i++;
            } else {

                if(Mathf.Sign(points[i]) != Mathf.Sign(points[i + 1])) {
                    //find zero with secant
                    float x1 = i / 10.0f * range;
                    float x2 = (x1 + ((i + 1) / 10.0f * range)) * 0.5f;

                    float y1 = Derivative(x1);
                    float y2 = Derivative(x2);
                    int id = 0;
                    while (true) {
                        id++;
                        if(id > 100) {
                            break;
                        }

                        if (Mathf.Abs(y2) < 0.000001f || Mathf.Abs(y2 - y1) < 0.000001f) {
                            break;
                        }

                        float d = (x2 - x1) / (y2 - y1);
                        var x2_ = x2;
                        
                        x2 = x1 - Derivative(x1) * d;
                        x1 = x2_;

                        y1 = Derivative(x1);
                        y2 = Derivative(x2);
                    }

                    result.Add(x2);


                }
            }
        }
        return result;
    }

    private void OnDrawGizmos()
    {
        Gizmos.matrix = transform.localToWorldMatrix;

        var extremums = FindExtremums(4.0f);

        var maxPoint = extremums.Max();
        var minPoint = extremums.Min();

        foreach (var ext in extremums) {
            Gizmos.color = Color.red;
            Gizmos.DrawLine(new Vector3(ext, 0, 0), new Vector3(ext, 100, 0));
        }

        Gizmos.color = Color.white;
        List<Vector3> oldPlot = new List<Vector3>();

      
        for (int j = 0; j < latSamples; ++j) {
           // Gizmos.matrix = Matrix4x4.Rotate(Quaternion.AngleAxis(360/20*j, Vector3.up)) * transform.localToWorldMatrix;
            Vector3 last = Vector3.zero;
           
            for (int i = 0; i <= lonSamples; ++i) {
                float x = (i / (float)lonSamples - 0.5f) * 2.0f * 4.0f;
                float val = Fx(x);
                Gizmos.color = (Color.Lerp(Color.red, Color.green, (val - minPoint) / (maxPoint - minPoint)));

                Vector3 current = Quaternion.AngleAxis(360 / (float)latSamples * j, Vector3.up) * new Vector3(x, val, 0);

                if(oldPlot.Count <= i) {
                    oldPlot.Add(current);
                } else {
                   
                    Gizmos.DrawLine(oldPlot[i], current);
                    oldPlot[i] = current;
                }
                if (i != 0) {
                    Gizmos.DrawLine(last, current);
                }
                last = current;
            }
        }

        Gizmos.matrix = transform.localToWorldMatrix;
        if (Sampler != null) {
            float sagitta = Sagitta(Sampler.transform.position.x);
            float derivative = Derivative(Sampler.transform.position.x);
            Gizmos.DrawWireSphere(new Vector3(Sampler.transform.position.x, sagitta, 0), 0.1f);
            Gizmos.color = Color.red;
            Gizmos.DrawLine(new Vector3(Sampler.transform.position.x, sagitta, 0), new Vector3(Sampler.transform.position.x, sagitta, 0) + new Vector3(1, derivative, 0) * 0.5f);

            Gizmos.color = Color.blue;
            Gizmos.DrawLine(new Vector3(Sampler.transform.position.x, sagitta, 0), new Vector3(Sampler.transform.position.x, sagitta, 0) + Normal(new Vector3(Sampler.transform.position.x, sagitta, 0)));
            //float sagitta = Sagitta(Sampler.transform.position.x);
        }


        if(PlotTraceRay != null) {
            Vector3 intersection = Vector3.zero;
            if(Intersect(new Ray(PlotTraceRay.transform.position, PlotTraceRay.transform.forward), ref intersection)) {
                Gizmos.DrawWireCube(intersection, Vector3.one * 0.1f);
            }
        }

    }
}