*
This commit is contained in:
@@ -0,0 +1,281 @@
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user