829 lines
31 KiB
C#
829 lines
31 KiB
C#
using System;
|
|
using System.Linq;
|
|
using System.Reflection;
|
|
|
|
namespace JD.Serialization
|
|
{
|
|
[AttributeUsage(AttributeTargets.All, Inherited = false)]
|
|
public class SplineUsageMode : System.Attribute
|
|
{
|
|
private Spline.UsageMode m_val;
|
|
|
|
public Spline.UsageMode Val
|
|
{
|
|
get { return m_val; }
|
|
set { m_val = value; }
|
|
}
|
|
|
|
public SplineUsageMode(Spline.UsageMode val)
|
|
{
|
|
m_val = val;
|
|
}
|
|
}
|
|
|
|
[AttributeUsage(AttributeTargets.All, Inherited = false, AllowMultiple = true)]
|
|
public class SplineUsageModeExt : System.Attribute
|
|
{
|
|
private Spline.UsageMode m_val;
|
|
|
|
public Spline.UsageMode Val
|
|
{
|
|
get { return m_val; }
|
|
set { m_val = value; }
|
|
}
|
|
|
|
public SplineUsageModeExt(Spline.UsageMode val)
|
|
{
|
|
m_val = val;
|
|
}
|
|
}
|
|
|
|
public partial class Spline
|
|
{
|
|
//-----------------------------------------------------------------------------------------
|
|
const float MTH_EPSILON = 0.00001f;
|
|
|
|
//-----------------------------------------------------------------------------------------
|
|
public enum UsageMode
|
|
{
|
|
X = 0, Y, Z,
|
|
XY,
|
|
XYZ,
|
|
WZ,
|
|
RGB
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------------------
|
|
public enum BoundsExceed
|
|
{
|
|
exceed_min,
|
|
exceed_max,
|
|
exceed_ok,
|
|
exceed_nullinterval
|
|
};
|
|
|
|
//-----------------------------------------------------------------------------------------
|
|
private float delta_t;
|
|
|
|
|
|
//-----------------------------------------------------------------------------------------
|
|
// Constructor
|
|
//
|
|
public Spline()
|
|
{
|
|
delta_t = 0;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------------------
|
|
// empty spline
|
|
//
|
|
public void Clear()
|
|
{
|
|
Points.Clear();
|
|
delta_t = 0;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------------------
|
|
// add a new point on spline
|
|
//
|
|
public void AddSplinePoint(Vec3d v, Vec3d normalIn, Vec3d normalInTime, Vec3d normalOut, Vec3d normalOutTime, SplinePoint.Interpolation_Enum _interp, float _t)
|
|
{
|
|
SplinePoint sp = new SplinePoint
|
|
{
|
|
Point = v,
|
|
NormalIn = normalIn,
|
|
NormalInTime = normalInTime,
|
|
NormalOut = normalOut,
|
|
NormalOutTime = normalOutTime,
|
|
Time = _t,
|
|
Interpolation = _interp
|
|
};
|
|
AddSplinePoint(sp);
|
|
}
|
|
|
|
public void AddSplinePoint(SplinePoint _splinePoint)
|
|
{
|
|
Points.Add(_splinePoint);
|
|
delta_t = (float)1 / (float)Points.Count();
|
|
}
|
|
|
|
//=========================================================================================
|
|
//
|
|
// Accessors / misc functions
|
|
//
|
|
//=========================================================================================
|
|
|
|
//-----------------------------------------------------------------------------------------
|
|
public int GetNumPoints() { return Points.Count(); }
|
|
//-----------------------------------------------------------------------------------------
|
|
public Vec3d GetValueAt(int _index) { return Points[_index].Point; }
|
|
public float getTimeAt(int _index) { return Points[_index].Time; }
|
|
public void RemoveAt(int _index)
|
|
{
|
|
if (_index < 0)
|
|
return;
|
|
if (_index >= Points.Count)
|
|
return;
|
|
Points.RemoveAt(_index);
|
|
}
|
|
|
|
public void setTimeValueAt(int _index, float _time, Vec3d _val)
|
|
{
|
|
SplinePoint pt = Points[_index];
|
|
pt.Point = _val;
|
|
pt.Time = _time;
|
|
Points[_index] = pt;
|
|
}
|
|
|
|
public SplinePoint getSplinePointAt(int _index) { return Points[_index]; }
|
|
public void setSplinePointAt(int _index, SplinePoint _sp) { Points[_index] = _sp; }
|
|
|
|
//-----------------------------------------------------------------------------------------
|
|
// sort points per frame
|
|
//
|
|
public void sortByTime()
|
|
{
|
|
int size = Points.Count();
|
|
for (int i = 0; i < size; i++)
|
|
{
|
|
float t = Points[i].Time;
|
|
int swapIndex = i;
|
|
for (int j = i + 1; j < size; j++)
|
|
{
|
|
if (t > Points[j].Time)
|
|
{
|
|
swapIndex = j;
|
|
t = Points[j].Time;
|
|
}
|
|
}
|
|
// do swap
|
|
if (i != swapIndex)
|
|
{
|
|
SplinePoint p = Points[i];
|
|
Points[i] = Points[swapIndex];
|
|
Points[swapIndex] = p;
|
|
}
|
|
}
|
|
}
|
|
|
|
public float GetMaxTime()
|
|
{
|
|
int size = Points.Count();
|
|
if (size == 0)
|
|
return 0;
|
|
float res = Points[0].Time;
|
|
for (int i = 1; i < size; i++)
|
|
{
|
|
if (res < Points[i].Time)
|
|
res = Points[i].Time;
|
|
}
|
|
return res;
|
|
}
|
|
|
|
public float GetMinTime()
|
|
{
|
|
int size = Points.Count();
|
|
if (size == 0)
|
|
return 0;
|
|
float res = Points[0].Time;
|
|
for (int i = 1; i < size; i++)
|
|
{
|
|
if (res > Points[i].Time)
|
|
res = Points[i].Time;
|
|
}
|
|
return res;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------------------
|
|
private BoundsExceed clampIndex(ref int _val, int _max)
|
|
{
|
|
if (_val < 0)
|
|
{
|
|
_val = 0;
|
|
return BoundsExceed.exceed_min;
|
|
}
|
|
if (_val > _max)
|
|
{
|
|
_val = _max;
|
|
return BoundsExceed.exceed_max;
|
|
}
|
|
return BoundsExceed.exceed_ok;
|
|
}
|
|
|
|
//=========================================================================================
|
|
//
|
|
// Spline / constant functions
|
|
//
|
|
//=========================================================================================
|
|
public static bool isDiff(Spline _a, Spline _b)
|
|
{
|
|
if (_a.Points.Count != _b.Points.Count)
|
|
return true;
|
|
if (Math.Abs(_a.TimeLoop - _b.TimeLoop) > float.Epsilon)
|
|
return true;
|
|
if (_a.TimeLoopMode != _b.TimeLoopMode)
|
|
return true;
|
|
for (int i = 0; i < _a.Points.Count; ++i)
|
|
{
|
|
if (SplinePoint.isDiff(_a.Points[i], _b.Points[i]))
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
public static bool isSame(Spline _a, Spline _b)
|
|
{
|
|
return !isSame(_a, _b);
|
|
}
|
|
|
|
public partial class SplinePoint
|
|
{
|
|
public SplinePoint()
|
|
{
|
|
}
|
|
|
|
public SplinePoint(SplinePoint _p)
|
|
{
|
|
FieldInfo[] myObjectFields = typeof(Spline.SplinePoint).GetFields(BindingFlags.NonPublic | BindingFlags.Public | BindingFlags.Instance);
|
|
foreach (FieldInfo fi in myObjectFields)
|
|
{
|
|
if (fi.FieldType == typeof(Vec3d))
|
|
{
|
|
fi.SetValue(this, new Vec3d((Vec3d)(fi.GetValue(_p))));
|
|
}
|
|
else
|
|
fi.SetValue(this, fi.GetValue(_p));
|
|
}
|
|
}
|
|
|
|
public static bool isDiff(SplinePoint _a, SplinePoint _b)
|
|
{
|
|
return _a.Interpolation != _b.Interpolation
|
|
|| Vec3d.DiffEpsilon(_a.NormalIn, _b.NormalIn)
|
|
|| Vec3d.DiffEpsilon(_a.NormalInTime, _b.NormalInTime)
|
|
|| Vec3d.DiffEpsilon(_a.NormalOut, _b.NormalOut)
|
|
|| Vec3d.DiffEpsilon(_a.NormalOutTime, _b.NormalOutTime)
|
|
|| Vec3d.DiffEpsilon(_a.Point, _b.Point)
|
|
|| Math.Abs(_a.Time - _b.Time) > float.Epsilon;
|
|
}
|
|
public static bool isSame(SplinePoint _a, SplinePoint _b)
|
|
{
|
|
return !isDiff(_a, _b);
|
|
}
|
|
|
|
#region Point Conversion
|
|
|
|
// Since we cannot assign a variable in a struct (ex: point.NormalOut = 12),
|
|
// The following code helps to convert existing code that considered the vectors were classes.
|
|
|
|
public float PointX
|
|
{
|
|
get { return _point.X; }
|
|
set { _point.X = value; }
|
|
}
|
|
public float PointY
|
|
{
|
|
get { return _point.Y; }
|
|
set { _point.Y = value; }
|
|
}
|
|
public float PointZ
|
|
{
|
|
get { return _point.Z; }
|
|
set { _point.Z = value; }
|
|
}
|
|
|
|
public float NormalInX
|
|
{
|
|
get { return _normalIn.X; }
|
|
set { _normalIn.X = value; }
|
|
}
|
|
public float NormalInY
|
|
{
|
|
get { return _normalIn.Y; }
|
|
set { _normalIn.Y = value; }
|
|
}
|
|
public float NormalInZ
|
|
{
|
|
get { return _normalIn.Z; }
|
|
set { _normalIn.Z = value; }
|
|
}
|
|
|
|
public float NormalOutX
|
|
{
|
|
get { return _normalOut.X; }
|
|
set { _normalOut.X = value; }
|
|
}
|
|
public float NormalOutY
|
|
{
|
|
get { return _normalOut.Y; }
|
|
set { _normalOut.Y = value; }
|
|
}
|
|
public float NormalOutZ
|
|
{
|
|
get { return _normalOut.Z; }
|
|
set { _normalOut.Z = value; }
|
|
}
|
|
|
|
public float NormalOutTimeX
|
|
{
|
|
get { return _normalOutTime.X; }
|
|
set { _normalOutTime.X = value; }
|
|
}
|
|
public float NormalOutTimeY
|
|
{
|
|
get { return _normalOutTime.Y; }
|
|
set { _normalOutTime.Y = value; }
|
|
}
|
|
public float NormalOutTimeZ
|
|
{
|
|
get { return _normalOutTime.Z; }
|
|
set { _normalOutTime.Z = value; }
|
|
}
|
|
|
|
public float NormalInTimeX
|
|
{
|
|
get { return _normalInTime.X; }
|
|
set { _normalInTime.X = value; }
|
|
}
|
|
public float NormalInTimeY
|
|
{
|
|
get { return _normalInTime.Y; }
|
|
set { _normalInTime.Y = value; }
|
|
}
|
|
public float NormalInTimeZ
|
|
{
|
|
get { return _normalInTime.Z; }
|
|
set { _normalInTime.Z = value; }
|
|
}
|
|
|
|
#endregion
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------------------
|
|
private bool constant_interpolatePosition(ref Vec3d _position, int _pt1, int _pt2, float _timeRatio)
|
|
{
|
|
_position = Points[(_timeRatio >= 1.0f) ? _pt2 : _pt1].Point;
|
|
return true;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------------------
|
|
private bool constant_interpolateTangent(SplinePoint _splinePoint, int _pt1, int _pt2, float _timeRatio)
|
|
{
|
|
_splinePoint.NormalIn.setNull();
|
|
_splinePoint.NormalInTime.setNull();
|
|
_splinePoint.NormalOut.setNull();
|
|
_splinePoint.NormalOutTime.setNull();
|
|
return true;
|
|
}
|
|
|
|
//=========================================================================================
|
|
//
|
|
// Spline / linear functions
|
|
//
|
|
//=========================================================================================
|
|
|
|
//-----------------------------------------------------------------------------------------
|
|
private bool linear_interpolatePosition(ref Vec3d _position, int _pt1, int _pt2, float _timeRatio)
|
|
{
|
|
if (_timeRatio >= 1.0f) _timeRatio = 1.0f;
|
|
_position = Points[_pt1].Point + (Points[_pt2].Point - Points[_pt1].Point) * _timeRatio;
|
|
return true;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------------------
|
|
private bool linear_interpolateTangent(SplinePoint _splinePoint, int _pt1, int _pt2, float _timeRatio)
|
|
{
|
|
_splinePoint.NormalIn.setNull();
|
|
_splinePoint.NormalInTime.setNull();
|
|
_splinePoint.NormalOut.setNull();
|
|
_splinePoint.NormalOutTime.setNull();
|
|
return true;
|
|
}
|
|
|
|
//=========================================================================================
|
|
//
|
|
// Spline / spline functions
|
|
//
|
|
//=========================================================================================
|
|
|
|
//-----------------------------------------------------------------------------------------
|
|
private bool spline_interpolatePosition(ref Vec3d _position, int _pt1, int _pt2, float _timeRatio)
|
|
{
|
|
int p0 = _pt1 - 1;
|
|
clampIndex(ref p0, Points.Count() - 1);
|
|
int p3 = _pt2 + 1;
|
|
clampIndex(ref p3, Points.Count() - 1);
|
|
|
|
_position = Eq(_timeRatio, Points[p0].Point, Points[_pt1].Point, Points[_pt2].Point, Points[p3].Point);
|
|
return true;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------------------
|
|
private bool spline_interpolateTangent(SplinePoint _splinePoint, int _pt1, int _pt2, float _timeRatio)
|
|
{
|
|
_splinePoint.NormalIn.setNull();
|
|
_splinePoint.NormalInTime.setNull();
|
|
_splinePoint.NormalOut.setNull();
|
|
_splinePoint.NormalOutTime.setNull();
|
|
return true;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------------------
|
|
// Solve the Catmull-Rom parametric equation for a given time(t) and vector quadruple (p1,p2,p3,p4)
|
|
//
|
|
public Vec3d Eq(float t, Vec3d p1, Vec3d p2, Vec3d p3, Vec3d p4)
|
|
{
|
|
float t2 = t * t;
|
|
float t3 = t2 * t;
|
|
|
|
float b1 = .5f * (-t3 + 2 * t2 - t);
|
|
float b2 = .5f * (3 * t3 - 5 * t2 + 2);
|
|
float b3 = .5f * (-3 * t3 + 4 * t2 + t);
|
|
float b4 = .5f * (t3 - t2);
|
|
|
|
return (p1 * b1 + p2 * b2 + p3 * b3 + p4 * b4);
|
|
}
|
|
|
|
//=========================================================================================
|
|
//
|
|
// Spline / bezier standard functions
|
|
//
|
|
//=========================================================================================
|
|
|
|
//-----------------------------------------------------------------------------------------
|
|
private bool bezier3D_interpolatePosition(ref Vec3d _position, int _pt1, int _pt2, float _timeRatio)
|
|
{
|
|
Vec3d position1 = Points[_pt1].Point;
|
|
Vec3d position2 = Points[_pt2].Point;
|
|
Vec3d tangent1 = position1 + Points[_pt1].NormalOut;
|
|
Vec3d tangent2 = position2 - Points[_pt2].NormalIn;
|
|
|
|
_position = getBezierPosition(position1, tangent1, tangent2, position2, _timeRatio);
|
|
return true;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------------------
|
|
private bool bezier3D_interpolateTangent(SplinePoint _splinePoint, ref SplinePoint _splinePointPrev, ref SplinePoint _splinePointNext, int _pt1, int _pt2, float _timeRatio)
|
|
{
|
|
Vec3d A = Points[_pt1].Point;
|
|
Vec3d B = A + Points[_pt1].NormalOut;
|
|
Vec3d D = Points[_pt2].Point;
|
|
Vec3d C = D - Points[_pt2].NormalIn;
|
|
|
|
return bezier3D_interpolateTangent(_splinePoint, ref _splinePointPrev, ref _splinePointNext, A, B, C, D, _timeRatio);
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------------------
|
|
private bool bezier3D_interpolateTangent(SplinePoint _splinePoint, ref SplinePoint _splinePointPrev, ref SplinePoint _splinePointNext, Vec3d A, Vec3d B, Vec3d C, Vec3d D, float _timeRatio)
|
|
{
|
|
/* M = B + t * BC. */
|
|
Vec3d M = B + _timeRatio * (C - B);
|
|
|
|
/* L1 = A + t * AB. */
|
|
Vec3d L1 = A + _timeRatio * (B - A);
|
|
|
|
/* R2 = C + t * CD. */
|
|
Vec3d R2 = C + _timeRatio * (D - C);
|
|
|
|
/* L2 = L1 + t * L1M. */
|
|
Vec3d L2 = L1 + _timeRatio * (M - L1);
|
|
|
|
/* R1 = M + t * MR2. */
|
|
Vec3d R1 = M + _timeRatio * (R2 - M);
|
|
|
|
_splinePointPrev.NormalOut = L1 - A;
|
|
_splinePoint.NormalIn = _splinePoint.Point - L2;
|
|
_splinePoint.NormalOut = R1 - _splinePoint.Point;
|
|
_splinePointNext.NormalIn = D - R2;
|
|
|
|
return true;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------------------
|
|
public Vec3d getBezierPosition(Vec3d p0, Vec3d p1, Vec3d p2, Vec3d p3, float t)
|
|
{
|
|
float tInv = 1 - t;
|
|
return (p0 * tInv * tInv * tInv) + (p1 * 3 * t * tInv * tInv) + (p2 * 3 * t * t * tInv) + (p3 * t * t * t);
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------------------
|
|
public float getBezierX(float x0, float x1, float x2, float x3, float t)
|
|
{
|
|
float Omt = 1 - t;
|
|
return (((((x0 * Omt) + (3 * t * x1)) * Omt) + (3 * t * t * x2)) * Omt) + (x3 * t * t * t);
|
|
}
|
|
|
|
//=========================================================================================
|
|
//
|
|
// Spline / bezier time parametric functions
|
|
//
|
|
//=========================================================================================
|
|
|
|
public void bezierTime_convertToBezier(SplinePoint _point0, SplinePoint _point1, Vec3d.axis _axis, ref Vec3d _p0, ref Vec3d _p1, ref Vec3d _p2, ref Vec3d _p3)
|
|
{
|
|
float x0 = _point0.Point.getValue(_axis);
|
|
float x1 = _point1.Point.getValue(_axis);
|
|
|
|
_p0.set(_point0.Time, x0);
|
|
_p1.set(_point0.Time + _point0.NormalOutTime.getValue(_axis), x0 + _point0.NormalOut.getValue(_axis));
|
|
_p2.set(_point1.Time - _point1.NormalInTime.getValue(_axis), x1 - _point1.NormalIn.getValue(_axis));
|
|
_p3.set(_point1.Time, x1);
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------------------
|
|
public float bezierTime_getTimeForX(Vec3d _p0, Vec3d _p1, Vec3d _p2, Vec3d _p3, float _x)
|
|
{
|
|
float tBegin = 0;
|
|
float tEnd = 1;
|
|
float tMid = 0;
|
|
|
|
float x0 = _p0.X;
|
|
float x1 = _p1.X;
|
|
float x2 = _p2.X;
|
|
float x3 = _p3.X;
|
|
float xCur;
|
|
|
|
for (int counter = 0; counter < 20; counter++)
|
|
{
|
|
tMid = (tEnd + tBegin) / 2;
|
|
|
|
xCur = getBezierX(x0, x1, x2, x3, tMid);
|
|
if (Math.Abs(xCur - _x) < 0.001f)
|
|
break;
|
|
|
|
if (xCur < _x)
|
|
tBegin = tMid;
|
|
else
|
|
tEnd = tMid;
|
|
}
|
|
|
|
return tMid;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------------------
|
|
public float getBezierY(Vec3d p1, Vec3d p2, Vec3d p3, Vec3d p4, float _x)
|
|
{
|
|
float tBegin = 0;
|
|
float tEnd = 1;
|
|
float tMid;
|
|
int counter;
|
|
Vec3d pos = new Vec3d();
|
|
|
|
for (counter = 0; counter < 20; counter++)
|
|
{
|
|
tMid = (tEnd + tBegin) / 2;
|
|
pos = getBezierPosition(p1, p2, p3, p4, tMid);
|
|
if (Math.Abs(pos.X - _x) < 0.001f)
|
|
break;
|
|
|
|
if (pos.X < _x)
|
|
{
|
|
tBegin = tMid;
|
|
}
|
|
else
|
|
{
|
|
tEnd = tMid;
|
|
}
|
|
}
|
|
return pos.Y;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------------------
|
|
private bool bezierTime_interpolatePosition(ref Vec3d _position, int _pt1, int _pt2, float _time)
|
|
{
|
|
Vec3d pos1 = Points[_pt1].Point;
|
|
Vec3d tgt1 = pos1 + Points[_pt1].NormalOut;
|
|
Vec3d pos2 = Points[_pt2].Point;
|
|
Vec3d tgt2 = pos2 - Points[_pt2].NormalIn;
|
|
|
|
float time1 = Points[_pt1].Time;
|
|
float time2 = Points[_pt2].Time;
|
|
|
|
Vec3d P0 = new Vec3d(time1, pos1.X, 0);
|
|
Vec3d P1 = new Vec3d(time1 + Points[_pt1].NormalOutTime.X, tgt1.X, 0);
|
|
Vec3d P2 = new Vec3d(time2 - Points[_pt2].NormalInTime.X, tgt2.X, 0);
|
|
Vec3d P3 = new Vec3d(time2, pos2.X, 0);
|
|
|
|
_position.X = getBezierY(P0, P1, P2, P3, _time);
|
|
|
|
P0.Y = pos1.Y;
|
|
P1.set(time1 + Points[_pt1].NormalOutTime.Y, tgt1.Y);
|
|
P2.set(time2 - Points[_pt2].NormalInTime.Y, tgt2.Y);
|
|
P3.Y = pos2.Y;
|
|
|
|
_position.Y = getBezierY(P0, P1, P2, P3, _time);
|
|
|
|
P0.Y = pos1.Z;
|
|
P1.set(time1 + Points[_pt1].NormalOutTime.Z, tgt1.Z);
|
|
P2.set(time2 - Points[_pt2].NormalInTime.Z, tgt2.Z);
|
|
P3.Y = pos2.Z;
|
|
|
|
_position.Z = getBezierY(P0, P1, P2, P3, _time);
|
|
|
|
return true;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------------------
|
|
private bool bezierTime_interpolateTangent(SplinePoint _splinePoint, ref SplinePoint _splinePointPrev, ref SplinePoint _splinePointNext, int _pt1, int _pt2, float _time)
|
|
{
|
|
Vec3d P0 = new Vec3d();
|
|
Vec3d P1 = new Vec3d();
|
|
Vec3d P2 = new Vec3d();
|
|
Vec3d P3 = new Vec3d();
|
|
float time;
|
|
|
|
Vec3d savePosition = new Vec3d(_splinePoint.Point);
|
|
Vec3d tangentOutPrev = new Vec3d();
|
|
Vec3d tangentTimeOutPrev = new Vec3d();
|
|
Vec3d tangentIn = new Vec3d();
|
|
Vec3d tangentTimeIn = new Vec3d();
|
|
Vec3d tangentOut = new Vec3d();
|
|
Vec3d tangentTimeOut = new Vec3d();
|
|
Vec3d tangentInNext = new Vec3d();
|
|
Vec3d tangentTimeInNext = new Vec3d();
|
|
|
|
foreach (Vec3d.axis _axis in Enum.GetValues(typeof(Vec3d.axis)))
|
|
{
|
|
bezierTime_convertToBezier(Points[_pt1], Points[_pt2], _axis, ref P0, ref P1, ref P2, ref P3);
|
|
time = bezierTime_getTimeForX(P0, P1, P2, P3, _time);
|
|
_splinePoint.Point.set(_time, getBezierX(P0.Y, P1.Y, P2.Y, P3.Y, time));
|
|
bezier3D_interpolateTangent(_splinePoint, ref _splinePointPrev, ref _splinePointNext, P0, P1, P2, P3, time);
|
|
tangentTimeOutPrev.setValue(_axis, _splinePointPrev.NormalOut.X);
|
|
tangentOutPrev.setValue(_axis, _splinePointPrev.NormalOut.Y);
|
|
tangentTimeIn.setValue(_axis, _splinePoint.NormalIn.X);
|
|
tangentIn.setValue(_axis, _splinePoint.NormalIn.Y);
|
|
tangentTimeOut.setValue(_axis, _splinePoint.NormalOut.X);
|
|
tangentOut.setValue(_axis, _splinePoint.NormalOut.Y);
|
|
tangentTimeInNext.setValue(_axis, _splinePointNext.NormalIn.X);
|
|
tangentInNext.setValue(_axis, _splinePointNext.NormalIn.Y);
|
|
}
|
|
|
|
_splinePoint.Point = savePosition;
|
|
_splinePoint.NormalIn = tangentIn;
|
|
_splinePoint.NormalOut = tangentOut;
|
|
_splinePoint.NormalInTime = tangentTimeIn;
|
|
_splinePoint.NormalOutTime = tangentTimeOut;
|
|
_splinePointNext.NormalIn = tangentInNext;
|
|
_splinePointNext.NormalInTime = tangentTimeInNext;
|
|
_splinePointPrev.NormalOut = tangentOutPrev;
|
|
_splinePointPrev.NormalOutTime = tangentTimeOutPrev;
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
//=========================================================================================
|
|
//
|
|
// main computing function
|
|
//
|
|
//=========================================================================================
|
|
|
|
//-----------------------------------------------------------------------------------------
|
|
private BoundsExceed getInfoAtTime(float _time, ref int _keyIndex, ref int _nextKeyIndex, ref float _timeRatio)
|
|
{
|
|
_timeRatio = 0;
|
|
_keyIndex = 0;
|
|
if (_time + MTH_EPSILON < Points[0].Time)
|
|
return BoundsExceed.exceed_min;
|
|
|
|
_keyIndex = Points.Count() - 1;
|
|
if (_time >= Points[_keyIndex].Time)
|
|
return BoundsExceed.exceed_max;
|
|
|
|
for (_keyIndex = 0; _keyIndex < Points.Count() - 1; _keyIndex++)
|
|
{
|
|
if (Points[_keyIndex + 1].Time >= _time)
|
|
break;
|
|
}
|
|
|
|
_nextKeyIndex = _keyIndex + 1;
|
|
clampIndex(ref _nextKeyIndex, Points.Count() - 1);
|
|
|
|
float deltaTime = Points[_nextKeyIndex].Time - Points[_keyIndex].Time;
|
|
if (deltaTime <= 0.0f)
|
|
{
|
|
_keyIndex = _nextKeyIndex;
|
|
return BoundsExceed.exceed_nullinterval;
|
|
}
|
|
|
|
_timeRatio = (_time - Points[_keyIndex].Time) / deltaTime;
|
|
if (_timeRatio < 0.0f) _timeRatio = 0.0f;
|
|
if (_timeRatio > 1.0f) _timeRatio = 1.0f;
|
|
|
|
return BoundsExceed.exceed_ok;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------------------
|
|
private Vec3d? interpolatePosition(float _time, int _pt1, int _pt2, float _timeRatio)
|
|
{
|
|
Vec3d position = new Vec3d();
|
|
switch (Points[_pt1].Interpolation)
|
|
{
|
|
case SplinePoint.Interpolation_Enum.interp_constant:
|
|
constant_interpolatePosition(ref position, _pt1, _pt2, _timeRatio);
|
|
return position;
|
|
case SplinePoint.Interpolation_Enum.interp_linear:
|
|
linear_interpolatePosition(ref position, _pt1, _pt2, _timeRatio);
|
|
return position;
|
|
case SplinePoint.Interpolation_Enum.interp_spline:
|
|
spline_interpolatePosition(ref position, _pt1, _pt2, _timeRatio);
|
|
return position;
|
|
case SplinePoint.Interpolation_Enum.interp_bezier_standard:
|
|
bezier3D_interpolatePosition(ref position, _pt1, _pt2, _timeRatio);
|
|
return position;
|
|
case SplinePoint.Interpolation_Enum.interp_bezier:
|
|
bezierTime_interpolatePosition(ref position, _pt1, _pt2, _time);
|
|
return position;
|
|
}
|
|
return null;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------------------
|
|
private bool interpolateTangent(SplinePoint _splinePoint, ref SplinePoint _splinePointPrev, ref SplinePoint _splinePointNext, float _time, int _pt1, int _pt2, float _timeRatio)
|
|
{
|
|
switch (Points[_pt1].Interpolation)
|
|
{
|
|
case SplinePoint.Interpolation_Enum.interp_constant: return constant_interpolateTangent(_splinePoint, _pt1, _pt2, _timeRatio);
|
|
case SplinePoint.Interpolation_Enum.interp_linear: return linear_interpolateTangent(_splinePoint, _pt1, _pt2, _timeRatio);
|
|
case SplinePoint.Interpolation_Enum.interp_spline: return spline_interpolateTangent(_splinePoint, _pt1, _pt2, _timeRatio);
|
|
case SplinePoint.Interpolation_Enum.interp_bezier_standard: return bezier3D_interpolateTangent(_splinePoint, ref _splinePointPrev, ref _splinePointNext, _pt1, _pt2, _timeRatio);
|
|
case SplinePoint.Interpolation_Enum.interp_bezier: return bezierTime_interpolateTangent(_splinePoint, ref _splinePointPrev, ref _splinePointNext, _pt1, _pt2, _time);
|
|
}
|
|
return false;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------------------
|
|
public Vec3d? interpolatePositionAtTime(float t, bool _constOutOfRange = false)
|
|
{
|
|
int p1 = 0;
|
|
int p2 = 0;
|
|
float timeRatio = 0;
|
|
|
|
if (TimeLoop > 0 && t > TimeLoop)
|
|
{
|
|
switch (TimeLoopMode)
|
|
{
|
|
case 1:
|
|
t = t % TimeLoop;
|
|
break;
|
|
case 2:
|
|
{
|
|
int v = (int)(t / TimeLoop);
|
|
if ((v & 1) == 1)
|
|
t = TimeLoop - (t % TimeLoop);
|
|
else
|
|
t = t % TimeLoop;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
BoundsExceed exceedBounds = getInfoAtTime(t, ref p1, ref p2, ref timeRatio);
|
|
if (exceedBounds != BoundsExceed.exceed_ok)
|
|
{
|
|
if (!_constOutOfRange)
|
|
return null;
|
|
if (Points.Count == 0)
|
|
return null;
|
|
switch (exceedBounds)
|
|
{
|
|
case BoundsExceed.exceed_min:
|
|
return new Vec3d(Points[0].Point);
|
|
case BoundsExceed.exceed_max:
|
|
return new Vec3d(Points[Points.Count - 1].Point);
|
|
case BoundsExceed.exceed_nullinterval:
|
|
return new Vec3d(Points[p1].Point);
|
|
default:
|
|
return null;
|
|
}
|
|
}
|
|
|
|
return interpolatePosition(t, p1, p2, timeRatio);
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------------------
|
|
public bool interpolateAtTime(float t, SplinePoint _splinePoint, ref SplinePoint _splinePointPrev, ref SplinePoint _splinePointNext)
|
|
{
|
|
int p1 = 0;
|
|
int p2 = 0;
|
|
float timeRatio = 0;
|
|
|
|
BoundsExceed exceedBounds = getInfoAtTime(t, ref p1, ref p2, ref timeRatio);
|
|
if (exceedBounds != BoundsExceed.exceed_ok)
|
|
{
|
|
_splinePoint.Point = new Vec3d(Points[p1].Point);
|
|
return false;
|
|
}
|
|
|
|
var pos = interpolatePosition(t, p1, p2, timeRatio);
|
|
if (pos == null)
|
|
return false;
|
|
|
|
_splinePoint.Point = pos.Value;
|
|
|
|
_splinePointPrev = Points[p1];
|
|
_splinePointNext = Points[p2];
|
|
return interpolateTangent(_splinePoint, ref _splinePointPrev, ref _splinePointNext, t, p1, p2, timeRatio);
|
|
}
|
|
}
|
|
}
|