├── README.md
├── c++
    ├── CatmullRom.cpp
    ├── CatmullRom.h
    ├── Vec3.cpp
    ├── Vec3.h
    └── test.cpp
├── curve1.PNG
├── curve2.PNG
├── js
    ├── CatmullRom.js
    ├── Vec3.js
    └── test.js
├── lua
    ├── CatmullRom.lua
    ├── Vec3.lua
    └── test.lua
└── matlab
    ├── CatmullRom.m
    ├── Vec3.m
    └── test.m


/README.md:
--------------------------------------------------------------------------------
 1 | # AverageCurve
 2 | 
 3 | #### 均速曲线路径
 4 | 
 5 | 由于贝塞尔曲线和基本样条曲线插值形成的曲线路径不是均速的，在某些场景可能会影响效果。比如说鱼游动，一般的曲线插值会忽快忽慢，像下面这样子:
 6 | 
 7 |   ![curve](https://chuantu.xyz/t6/734/1589618846x3070492176.png)
 8 |   
 9 |   变速的曲线不利于精确控制速度，所以需要做额外的计算，以符合要求。
10 |   
11 | 
12 | 
13 | 在插值之前，额外计算到指定长度的t变量，实现均速曲线，处理之后的效果图：
14 | 
15 |   ![curve](http://chuantu.xyz/t6/734/1589619484x992248267.png)
16 |   
17 |  #### 均速缓动曲线
18 |  
19 | 前言：
20 | 
21 | 物体的运动有时候需要由快变慢或者慢慢加快 这就需要叠加一个缓动曲线，游戏引擎预设的一些曲线都是初等函数叠加形成的，不能直观的感受到缓动曲线的变化趋势。
22 | 因此需要一个能够直接想象的到，并且方便自定义曲线的方式。
23 | 
24 | 实现：
25 | 
26 |   >  let rom = new CatmullRom(pts,1)  
27 |   >  rom.lerp(t).y
28 |   
29 | 
30 | <table>
31 |     <tr>
32 |         <td>接口</td>
33 |         <td>参数</td>
34 |         <td>说明</td>
35 |     </tr>
36 |     <tr>
37 |         <td rowspan="2">catmullRom=new CatmullRom(pts,usage)</td>
38 |     <td>pts</td>
39 |         <td>曲线经过的点数组</td>
40 |   </tr>
41 |     <tr>
42 |     <td>usage</td>
43 |         <td>0：均速路径曲线<br> 1：均速缓动曲线</td>
44 |     </tr>
45 |   <tr>
46 |         <td>catmullRom.lerp(t)</td>
47 |         <td>t</td>
48 |         <td>插件比例，范围0-1，0：起点， 1：终点</td>
49 |    </tr>
50 | </table>
51 | 
52 | 
53 | matlab版本可以直观的观察曲线的轨迹，可以用来编辑曲线。
54 | 


--------------------------------------------------------------------------------
/c++/CatmullRom.cpp:
--------------------------------------------------------------------------------
  1 | #include"CatmullRom.h"
  2 | 
  3 | CatmullRom::CatmullRom(const vector<Vec3>& pts, int usage)
  4 | {
  5 | 	points = pts;
  6 | 
  7 | 	int len = pts.size();
  8 | 	bClose = pts[0] == pts[len - 1];
  9 | 
 10 | 	float sumDistance = 0;
 11 | 	Vec3  p0, p1, p2, p3;
 12 | 	for (int i = 0; i < len - 1; i++){
 13 | 		if (i == 0) {
 14 | 			p0 = bClose ? pts[len - 2] : pts[0];
 15 | 		}
 16 | 		else {
 17 | 			p0 = pts[i - 1];
 18 | 		}
 19 | 
 20 | 		p1 = pts[i];
 21 | 		p2 = pts[i + 1];
 22 | 		if (i + 1 == len - 1) {
 23 | 			p3 = bClose ? pts[1] : pts[len - 1];
 24 | 		}
 25 | 		else {
 26 | 			p3 = pts[i + 2];
 27 | 		}
 28 | 
 29 | 		Funcs fs = CatmullRom::curve(p0, p1, p2, p3);
 30 | 		funcs.push_back(fs);
 31 | 		funcs[i].f2 = usage == 0 ? fs.f2 : fs.f3;
 32 | 
 33 | 		sumDistance += CatmullRom::gaussLegendre(funcs[i].f2, 0, 1);
 34 | 		distances.push_back(sumDistance);
 35 | 	}
 36 | }
 37 | 
 38 | CatmullRom::~CatmullRom()
 39 | {
 40 | }
 41 | 
 42 | 
 43 | Vec3 CatmullRom::lerp(float t){
 44 | 	int len = points.size();
 45 | 	//第一个和最后一个点直接返回
 46 | 	if (t == 0) {
 47 | 		return points[0];
 48 | 	}
 49 | 	else if (t == 1) {
 50 | 		return points[len - 1];
 51 | 	}
 52 | 	//平均距离
 53 | 	float averDis = t * distances[len - 2];
 54 | 	float index = 0, beyond = 0, percent = 0;
 55 | 	for (int i = 0; i < len - 1; i++) {
 56 | 		if (averDis < distances[i]) {
 57 | 			float preDis = i == 0 ? 0 : distances[i - 1];
 58 | 			index = i;
 59 | 			beyond = averDis - preDis;
 60 | 			percent = beyond / (distances[i] - preDis);
 61 | 			break;
 62 | 		}
 63 | 	}
 64 | 	//牛顿切线法求根
 65 | 	float a = percent, b;
 66 | 		//最多迭代6次
 67 | 	for (int i = 0; i < 6; i++) {
 68 | 		float actualLen = CatmullRom::gaussLegendre(funcs[index].f2, 0, a);
 69 | 		b = a - (actualLen - beyond) / funcs[index].f2(a);
 70 | 		if (abs(a - b) < 0.0001) {
 71 | 			break;
 72 | 		}
 73 | 		a = b;
 74 | 	}
 75 | 	percent = b;
 76 | 	return funcs[index].f1(percent);
 77 | }
 78 | CatmullRom::Funcs CatmullRom::curve(Vec3 p0, Vec3 p1, Vec3 p2, Vec3 p3){
 79 | 	// 弹性
 80 | 	float s = 0.5;
 81 | 	//计算三次样条线函数系数
 82 | 	Vec3 b1 = p0*(-s) + p1*(2 - s) + p2*(s - 2) + p3*s;
 83 | 	Vec3 b2 = p0 * 2 * s + p1*(s - 3) + p2*(3 - 2 * s) + p3*(-s);
 84 | 	Vec3 b3 = p0*(-s) + p2*s;
 85 | 	Vec3 b4 = p1;
 86 | 
 87 | 	// 函数曲线
 88 | 	auto f1 = [=](float x) -> Vec3 {
 89 | 		return b1*pow(x , 3) + b2*pow(x , 2) + b3*x + b4;
 90 | 	};
 91 | 	// 曲线长度变化率, 用于匀速曲线运动
 92 | 	auto f2 = [=](float x) -> float {
 93 | 			Vec3 der = b1 * 3 * pow(x, 2) + b2*x * 2 + b3;
 94 | 			return sqrt(pow(der.x, 2) + pow(der.y, 2) + pow(der.z, 2));
 95 | 	};
 96 | 	// 曲线x变化率, 用于自定义缓动曲线
 97 | 	auto f3 = [=](float x) -> float {
 98 | 		Vec3 der = b1 * 3 * pow(x, 2) + b2*x * 2 + b3;
 99 | 		return der.x;
100 | 	};
101 | 	return Funcs(f1, f2, f3);
102 | }
103 | 
104 | float CatmullRom::gaussLegendre(const function<float(float)>& func, float a, float  b){
105 | 	//3次系数
106 | 	static std::map<float, float> GauFactor{ { 0.7745966692, 0.555555556 }, { 0, 0.8888888889 } };
107 | 	//5次系数
108 | 	//static std::map<float, float> GauFactor{ { 0.9061798459, 0.2369268851 }, { 0.5384693101, 0.4786286705 }, { 0, 0.5688888889 } };
109 | 	float GauSum = 0;
110 | 	
111 | 	for (auto &kv : GauFactor) {
112 | 		float key = kv.first;
113 | 		float v = kv.second;
114 | 		float t = ((b - a) * key + a + b) / 2;
115 | 		float der = func(t);
116 | 		GauSum = GauSum + der * v;
117 | 		if (key > 0) {
118 | 			t = ((b - a) * (-key) + a + b) / 2;
119 | 			der = func(t);
120 | 			GauSum = GauSum + der * v;
121 | 		}
122 | 	}
123 | 	return GauSum * (b - a) / 2;
124 | }


--------------------------------------------------------------------------------
/c++/CatmullRom.h:
--------------------------------------------------------------------------------
 1 | #ifndef __CatmullRom__
 2 | #define __CatmullRom__
 3 | 
 4 | #include"Vec3.h"
 5 | #include <vector>
 6 | #include <functional>
 7 | #include <map>
 8 | 
 9 | using namespace std;
10 | class CatmullRom
11 | {
12 | public:
13 | 	// usage
14 | 	// 0 : 均速曲线运动, s变化恒定
15 | 	// 1 : 自定义缓动曲线, t变化恒定
16 | 	CatmullRom(const vector<Vec3>& pts, int usage = 0);
17 | 	~CatmullRom();
18 | 
19 | 	typedef struct _Funcs{
20 | 		function<Vec3(float)>  f1;
21 | 		function<float(float)> f2;
22 | 		function<float(float)> f3;
23 | 		_Funcs(const function<Vec3(float)>& f1, const function<float(float)>& f2, const function<float(float)>& f3) :f1(f1), f2(f2), f3(f3){};
24 | 	} Funcs;
25 | 
26 | 	//样条曲线经过的点
27 | 	vector<Vec3> points;
28 | 	//到每一个点的曲线长度
29 | 	vector<float> distances;
30 | 	//每一段函数曲线，导函数ds，dx
31 | 	vector<Funcs> funcs;
32 | 
33 | 	//是否环路
34 | 	bool bClose;
35 | 	//匀速插值
36 | 	Vec3 lerp(float t);
37 | 
38 | 	//基本样条线插值算法
39 | 	static Funcs curve(Vec3 p0, Vec3 p1, Vec3 p2, Vec3 p3);
40 | 	//高斯—勒让德积分公式可以用较少节点数得到高精度的计算结果
41 | 	static float gaussLegendre(const function<float(float)>& func, float a, float  b);
42 | private:
43 | 
44 | };
45 | 
46 | #endif  // __CatmullRom__


--------------------------------------------------------------------------------
/c++/Vec3.cpp:
--------------------------------------------------------------------------------
 1 | #include "Vec3.h"
 2 | 
 3 | Vec3::Vec3()
 4 | : x(0.0f), y(0.0f), z(0.0f)
 5 | {
 6 | }
 7 | 
 8 | Vec3::Vec3(float xx, float yy, float zz)
 9 | : x(xx), y(yy), z(zz)
10 | {
11 | }
12 | 
13 | Vec3::Vec3(const Vec3& copy)
14 | {
15 | 	x = copy.x;
16 | 	y = copy.y;
17 | 	z = copy.z;
18 | }
19 | 


--------------------------------------------------------------------------------
/c++/Vec3.h:
--------------------------------------------------------------------------------
 1 | #ifndef __Vec3__
 2 | #define __Vec3__
 3 | 
 4 | class Vec3{
 5 | public:
 6 | 	float x;
 7 | 	float y;
 8 | 	float z;
 9 | 
10 | 	Vec3();
11 | 	Vec3(const Vec3& copy);
12 | 	Vec3(float xx, float yy, float zz);
13 | 
14 | 	//重载运算符
15 | 	inline Vec3 Vec3::operator+(const Vec3& v) const
16 | 	{
17 | 		return Vec3(x + v.x, y + v.y, z + v.z);
18 | 	}
19 | 
20 | 	inline Vec3 Vec3::operator-(const Vec3& v) const
21 | 	{
22 | 		return Vec3(x - v.x, y - v.y, z - v.z);
23 | 	}
24 | 
25 | 	inline Vec3 Vec3::operator*(float s) const
26 | 	{
27 | 		return Vec3(x * s, y * s, z * s);
28 | 	}
29 | 
30 | 	inline bool Vec3::operator==(const Vec3& v) const
31 | 	{
32 | 		return x == v.x && y == v.y && z == v.z;
33 | 	}
34 | };
35 | #endif  // __Vec3__


--------------------------------------------------------------------------------
/c++/test.cpp:
--------------------------------------------------------------------------------
 1 | // test.cpp : 定义控制台应用程序的入口点。
 2 | //
 3 | 
 4 | #include"Vec3.h"
 5 | #include"CatmullRom.h"
 6 | 
 7 | using namespace std;
 8 | int main(int argc, char* argv[])
 9 | {
10 | 	std::vector<Vec3> points = { Vec3(0.1, 0.4, 0), Vec3(0.5, 0.4, 0), Vec3(0.7, 0.1, 0), Vec3(0.9, 0.9, 0), Vec3(0.1, 0.4, 0) };
11 | 
12 | 	CatmullRom rom(points);
13 | 
14 | 	std::vector<Vec3> ret;
15 | 	for (float i = 0; i <= 1; i += 0.2) {
16 | 		ret.push_back(rom.lerp(i));
17 | 	}
18 | 
19 | 	//=> ret
20 | 	return 0;
21 | }
22 | 
23 | 


--------------------------------------------------------------------------------
/curve1.PNG:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/zx6733090/AverageCurve/84ebce998f0d57a02c8c412170cd4bc7e062ee3b/curve1.PNG


--------------------------------------------------------------------------------
/curve2.PNG:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/zx6733090/AverageCurve/84ebce998f0d57a02c8c412170cd4bc7e062ee3b/curve2.PNG


--------------------------------------------------------------------------------
/js/CatmullRom.js:
--------------------------------------------------------------------------------
  1 | class CatmullRom {
  2 |     //CatmullRom 样条曲线
  3 |     //   给定一组控制点而得到一条曲线,曲线的大致形状由这些点控制
  4 |     constructor(pts, usage) {
  5 |         this.usage = usage || 0
  6 |         this.points = pts
  7 | 
  8 |         this.bClose = pts[0].eq(pts[pts.length - 1])
  9 |         this.funcs = []
 10 |         this.distances = []
 11 |         let sumDistance = 0
 12 |         let p0, p1, p2, p3
 13 |         for (let i = 0; i < pts.length - 1; i++) {
 14 |             if (i == 0) {
 15 |                 p0 = this.bClose ? pts[pts.length - 2] : pts[0]
 16 |             } else {
 17 |                 p0 = pts[i - 1]
 18 |             }
 19 | 
 20 |             p1 = pts[i]
 21 |             p2 = pts[i + 1]
 22 |             if (i + 1 == pts.length - 1) {
 23 |                 p3 = this.bClose ? pts[1] : pts[pts.length - 1]
 24 |             } else {
 25 |                 p3 = pts[i + 2]
 26 |             }
 27 | 
 28 |             this.funcs[i] = []
 29 |             let ds, dx;
 30 |             [this.funcs[i][0], ds, dx] = CatmullRom.curve(p0, p1, p2, p3);
 31 |             this.funcs[i][1] = this.usage == 0 ? ds : dx
 32 | 
 33 |             sumDistance += CatmullRom.gaussLegendre(this.funcs[i][1], 0, 1);
 34 |             this.distances[i] = sumDistance
 35 |         }
 36 |     }
 37 |     lerp(t) {
 38 |         //第一个和最后一个点直接返回
 39 |         if (t == 0) {
 40 |             return this.points[0]
 41 |         } else if (t == 1) {
 42 |             return this.points[this.points.length - 1]
 43 |         }
 44 |         //平均距离
 45 |         let averDis = t * this.distances[this.points.length - 2]
 46 |         let index = 0,
 47 |             beyond = 0,
 48 |             percent = 0;
 49 |         for (let i = 0; i < this.points.length - 1; i++) {
 50 |             if (averDis < this.distances[i]) {
 51 |                 let preDis = i == 0 ? 0 : this.distances[i - 1]
 52 |                 index = i
 53 |                 beyond = averDis - preDis;
 54 |                 percent = beyond / (this.distances[i] - preDis);
 55 |                 break
 56 |             }
 57 |         }
 58 |         //牛顿切线法求根
 59 |         let a = percent,
 60 |             b
 61 |         //最多迭代6次
 62 |         for (let i = 0; i < 6; i++) {
 63 |             let actualLen = CatmullRom.gaussLegendre(this.funcs[index][1], 0, a);
 64 |             b = a - (actualLen - beyond) / this.funcs[index][1](a)
 65 |             if (Math.abs(a - b) < 0.0001) {
 66 |                 break
 67 |             }
 68 |             a = b
 69 |         }
 70 |         percent = b
 71 |         return this.funcs[index][0](percent)
 72 |     }
 73 |     static curve(p0, p1, p2, p3) {
 74 |         //CatmullRom
 75 |         //   基本样条线插值算法
 76 |         // 弹性
 77 |         let s = 0.5
 78 |         //计算三次样条线函数系数
 79 |         let b1 = p0.mtimes(-s).plus(p1.mtimes(2 - s)).plus(p2.mtimes(s - 2)).plus(p3.mtimes(s))
 80 |         let b2 = p0.mtimes(2 * s).plus(p1.mtimes(s - 3)).plus(p2.mtimes(3 - 2 * s)).plus(p3.mtimes(-s))
 81 |         let b3 = p0.mtimes(-s).plus(p2.mtimes(s))
 82 |         let b4 = p1
 83 | 
 84 |         // 函数曲线
 85 |         function fx(x) {
 86 |             return b1.mtimes(Math.pow(x, 3)).plus(b2.mtimes(Math.pow(x, 2))).plus(b3.mtimes(x)).plus(b4)
 87 |         }
 88 |         // 曲线长度变化率,用于匀速曲线运动
 89 |         function ds(x) {
 90 |             let der = b1.mtimes(3 * Math.pow(x, 2)).plus(b2.mtimes(2 * x)).plus(b3)
 91 |             return Math.sqrt(Math.pow(der.x, 2) + Math.pow(der.y, 2) + Math.pow(der.z, 2))
 92 |         }
 93 |         // 曲线x变化率,用于自定义缓动曲线
 94 |         function dx(x) {
 95 |             let der = b1.mtimes(3 * Math.pow(x, 2)).plus(b2.mtimes(2 * x)).plus(b3)
 96 |             return der.x
 97 |         }
 98 |         return [fx, ds, dx]
 99 |     }
100 |     static gaussLegendre(func, a, b) {
101 |         //gaussLegendre 高斯—勒让德积分公式可以用较少节点数得到高精度的计算结果
102 |         // a     左区间
103 |         // b     右区间
104 |         //3次系数
105 |         let GauFactor = {
106 |             0.7745966692: 0.555555556,
107 |             0: 0.8888888889
108 |         }
109 |         //5次系数
110 |         //let GauFactor = {0.9061798459:0.2369268851,0.5384693101:0.4786286705,0:0.5688888889}
111 |         //积分
112 |         let GauSum = 0
113 |         for (let key in GauFactor) {
114 |             let v = GauFactor[key]
115 |             let t = ((b - a) * key + a + b) / 2
116 |             let der = func(t)
117 |             GauSum = GauSum + der * v
118 |             if (key > 0) {
119 |                 t = ((b - a) * (-key) + a + b) / 2
120 |                 der = func(t)
121 |                 GauSum = GauSum + der * v
122 |             }
123 |         }
124 |         return GauSum * (b - a) / 2
125 |     }
126 | }
127 | module.exports = CatmullRom


--------------------------------------------------------------------------------
/js/Vec3.js:
--------------------------------------------------------------------------------
 1 |  class Vec3 {
 2 |      //三维向量
 3 |      constructor(x, y, z) {
 4 |          if (z != undefined) {
 5 |              this.x = x
 6 |              this.y = y
 7 |              this.z = z
 8 |          } else if (x != undefined) {
 9 |              this.x = x.x
10 |              this.y = x.y
11 |              this.z = x.z
12 |          } else {
13 |              this.x = 0
14 |              this.y = 0
15 |              this.z = 0
16 |          }
17 |      }
18 |      //重载 == 运算符
19 |      eq(other) {
20 |          return this.x == other.x && this.y == other.y && this.z == other.z
21 |      }
22 |      //重载 - 运算符
23 |      minus(other) {
24 |          return new Vec3(this.x - other.x, this.y - other.y, this.z - other.z)
25 |      }
26 |      //重载 + 运算符
27 |      plus(other) {
28 |          if (typeof (other) == "number") {
29 |              return new Vec3(this.x + other, this.y + other, this.z + other)
30 |          } else {
31 |              return new Vec3(this.x + other.x, this.y + other.y, this.z + other.z)
32 |          }
33 |      }
34 |      //重载 * 运算符
35 |      mtimes(other) {
36 |          if (typeof (other) == "number") {
37 |              return new Vec3(this.x * other, this.y * other, this.z * other)
38 |          } else {
39 |              return new Vec3(this.x * other.x, this.y * other.y, this.z * other.z)
40 |          }
41 |      }
42 |      //重载 ^ 运算符
43 |      mpower(n) {
44 |          return new Vec3(Math.pow(this.x, n), Math.pow(this.y, n), Math.pow(this.z, n))
45 |      }
46 |  }
47 |  module.exports = Vec3


--------------------------------------------------------------------------------
/js/test.js:
--------------------------------------------------------------------------------
 1 | /**
 2 | curve(p0: cc.Vec2, p1, p2, p3) {
 3 |         let s = 0.5;
 4 |         let b1 = p0.mul(-s).add(p1.mul(2 - s)).add(p2.mul(s - 2)).add(p3.mul(s));
 5 |         let b2 = p0.mul(2 * s).add(p1.mul(s - 3)).add(p2.mul(3 - 2 * s)).add(p3.mul(-s));
 6 |         let b3 = p0.mul(-s).add(p2.mul(s));
 7 |         let b4 = p1;
 8 |         function fx(x) {
 9 |             return b1.mul(Math.pow(x, 3)).add(b2.mul(Math.pow(x, 2))).add(b3.mul(x)).add(b4)
10 |         }
11 |         return fx;
12 |     }
13 | */
14 | let Vec3 = require('./Vec3')
15 | let CatmullRom = require('./CatmullRom') 
16 | 
17 | let points = [new Vec3(0.1, 0.4, 0), new Vec3(0.5, 0.4, 0), new Vec3(0.7, 0.1, 0), new Vec3(0.9, 0.9, 0), new Vec3(0.1, 0.4, 0)]
18 | 
19 | let rom = new CatmullRom(points)
20 | 
21 | let ret = []
22 | for (let i = 0; i <= 1; i += 0.2) {
23 |     ret.push(rom.lerp(i))
24 | }
25 | console.log(ret)
26 | 


--------------------------------------------------------------------------------
/lua/CatmullRom.lua:
--------------------------------------------------------------------------------
  1 | local CatmullRom;
  2 | 
  3 | CatmullRom = {
  4 |         -- 构造式
  5 |         __call = function(self, pts, usage)
  6 |             self.usage = usage or 0
  7 |             self.points = pts
  8 |             self.bClose = pts[1] == pts[#pts]
  9 |             
 10 |             self.funcs = {}
 11 |             self.distances = {}
 12 |             local sumDistance = 0
 13 |             local p0, p1, p2, p3
 14 |             for i = 1, #pts - 1 do
 15 |                 if i == 1 then
 16 |                     p0 = self.bClose and pts[#pts - 1] or pts[1]
 17 |                 else
 18 |                     p0 = pts[i - 1]
 19 |                 end
 20 |                 
 21 |                 p1 = pts[i]
 22 |                 p2 = pts[i + 1]
 23 |                 if i + 1 == #pts then
 24 |                     p3 = self.bClose and pts[2] or pts[#pts]
 25 |                 else
 26 |                     p3 = pts[i + 2]
 27 |                 end
 28 |                 
 29 |                 self.funcs[i] = {}
 30 |                 local ds, dx;
 31 |                 self.funcs[i][1], ds, dx = CatmullRom.curve(p0, p1, p2, p3)
 32 |                 self.funcs[i][2] = self.usage == 0 and ds or dx
 33 |                 
 34 |                 sumDistance = sumDistance + CatmullRom.gaussLegendre(self.funcs[i][2], 0, 1)
 35 |                 self.distances[i] = sumDistance
 36 |             end
 37 |             return self
 38 |         end,
 39 |         lerp = function(self, t)
 40 |             --第一个和最后一个点直接返回
 41 |             if t == 0 then
 42 |                 return self.points[1]
 43 |             elseif t == 1 then
 44 |                 return self.points[#self.points]
 45 |             end
 46 |             
 47 |             --平均距离
 48 |             local averDis = t * self.distances[#self.points - 1]
 49 |             local index, beyond, percent = 0, 0, 0
 50 |             for i = 1, #self.points - 1 do
 51 |                 if averDis < self.distances[i] then
 52 |                     local preDis = i == 1 and 0 or self.distances[i - 1]
 53 |                     index = i
 54 |                     beyond = averDis - preDis
 55 |                     percent = beyond / (self.distances[i] - preDis)
 56 |                     break
 57 |                 end
 58 |             end
 59 |             --牛顿切线法求根
 60 |             local a, b = percent
 61 |             --最多迭代6次
 62 |             for i = 1, 6 do
 63 |                 local actualLen = CatmullRom.gaussLegendre(self.funcs[index][2], 0, a)
 64 |                 b = a - (actualLen - beyond) / self.funcs[index][2](a)
 65 |                 if math.abs(a - b) < 0.0001 then
 66 |                     break
 67 |                 end
 68 |                 a = b
 69 |             end
 70 |             percent = b
 71 |             return self.funcs[index][1](percent)
 72 |         end,
 73 |         curve = function(p0, p1, p2, p3)
 74 |             --CatmullRom
 75 |             --   基本样条线插值算法
 76 |             -- 弹性
 77 |             local s = 0.5
 78 |             --计算三次样条线函数系数
 79 |             local b1 = -s * p0 + (2 - s) * p1 + (s - 2) * p2 + s * p3
 80 |             local b2 = 2 * s * p0 + (s - 3) * p1 + (3 - 2 * s) * p2 + (-s) * p3
 81 |             local b3 = -s * p0 + s * p2
 82 |             local b4 = p1
 83 |             -- 函数曲线
 84 |             local function fx(x)
 85 |                 return b1 * x ^ 3 + b2 * x ^ 2 + b3 * x + b4
 86 |             end
 87 |             -- 曲线长度变化率,用于匀速曲线运动
 88 |             local function ds(x)
 89 |                 local der = 3 * b1 * x ^ 2 + 2 * b2 * x + b3
 90 |                 return math.sqrt(der.x ^ 2 + der.y ^ 2 + der.z ^ 2)
 91 |             end
 92 |             -- 曲线x变化率,用于自定义缓动曲线
 93 |             local function dx(x)
 94 |                 local der = 3 * b1 * x ^ 2 + 2 * b2 * x + b3
 95 |                 return der.x
 96 |             end
 97 |             return fx, ds, dx
 98 |         end,
 99 |         gaussLegendre = function(func, a, b)
100 |             --gaussLegendre 高斯—勒让德积分公式可以用较少节点数得到高精度的计算结果
101 |             -- a     左区间
102 |             -- b     右区间
103 |             -- 3次系数
104 |             local GauFactor = {
105 |                 [0.7745966692] = 0.555555556,
106 |                 [0] = 0.8888888889
107 |             }
108 |             --5次系数
109 |             --local GauFactor = {0.9061798459=0.2369268851,0.5384693101=0.4786286705,0=0.5688888889}
110 |             --积分
111 |             local GauSum = 0
112 |             for key, v in pairs(GauFactor) do
113 |                 local t = ((b - a) * key + a + b) / 2
114 |                 local der = func(t)
115 |                 GauSum = GauSum + der * v
116 |                 if key > 0 then
117 |                     t = ((b - a) * (-key) + a + b) / 2
118 |                     der = func(t)
119 |                     GauSum = GauSum + der * v
120 |                 end
121 |             end
122 |             return GauSum * (b - a) / 2
123 |         end
124 | }
125 | -- 索引元表
126 | CatmullRom.__index = CatmullRom
127 | return function(pts, usage)
128 |     local ret = {}
129 |     setmetatable(ret, CatmullRom)
130 |     return ret(pts, usage)
131 | end


--------------------------------------------------------------------------------
/lua/Vec3.lua:
--------------------------------------------------------------------------------
 1 | local vec3 ;
 2 | vec3= {
 3 |         -- 构造式
 4 |         __call = function(self, x, y, z)
 5 |             if z ~= nil then
 6 |                 self.x = x
 7 |                 self.y = y
 8 |                 self.z = z
 9 |             elseif x ~= nil then
10 |                 self.x = x.x
11 |                 self.y = x.y
12 |                 self.z = x.z
13 |             else
14 |                 self.x = 0
15 |                 self.y = 0
16 |                 self.z = 0
17 |             end
18 |             return self
19 |         end,
20 |         --重载 == 运算符
21 |         __eq = function(a, b)
22 |             return a.x == b.x and a.y == b.y and a.z == b.z
23 |         end,
24 |         --重载 - 运算符
25 |         __sub = function(a, b)
26 |             local obj = {}
27 |             setmetatable(obj, vec3)
28 |             if type(b) == "number" then
29 |                 return obj(a.x - b , a.y - b , a.z - b)
30 |             else
31 |                 return obj(a.x - b.x , a.y - b.y , a.z - b.z)
32 |             end
33 |         end,
34 |         --重载 + 运算符
35 |         __add = function(a, b)
36 |             local obj = {}
37 |             setmetatable(obj, vec3)
38 |             if type(a) == "number" then
39 |                 return obj(a + b.x , a + b.y , a + b.z)
40 |             elseif type(b) == "number" then 
41 |                 return obj(a.x + b , a.y + b , a.z +b)
42 |             else
43 |                 return obj(a.x + b.x , a.y + b.y , a.z +b.z)
44 |             end
45 |         end,
46 |         --重载 * 运算符
47 |         __mul=function(a, b)
48 |             local obj = {}
49 |             setmetatable(obj, vec3)
50 |             if type(a) == "number" then
51 |                 return obj(a * b.x , a * b.y , a * b.z)
52 |             elseif type(b) == "number" then 
53 |                 return obj(a.x * b , a.y * b , a.z *b)
54 |             else
55 |                 return obj(a.x * b.x , a.y * b.y , a.z *b.z)
56 |             end
57 |         end,
58 |         --重载 ^ 运算符
59 |         __pow=function(a, n)
60 |             local obj = {}
61 |             setmetatable(obj, vec3)
62 |             return obj(math.pow(a.x ,n) , math.pow(a.y ,n) , math.pow(a.z ,n))
63 |         end
64 | }
65 | return function(x, y, z)
66 |     local ret = {}
67 |     setmetatable(ret, vec3)
68 |     return ret(x, y, z)
69 | end
70 | 


--------------------------------------------------------------------------------
/lua/test.lua:
--------------------------------------------------------------------------------
 1 | local Vec3 = require('Vec3')
 2 | local CatmullRom = require('./CatmullRom')
 3 | 
 4 | local points = {Vec3(0.1, 0.4, 0), Vec3(0.5, 0.4, 0), Vec3(0.7, 0.1, 0), Vec3(0.9, 0.9, 0), Vec3(1, 0.4, 0)}
 5 | 
 6 | local rom = CatmullRom(points)
 7 | 
 8 | local ret = {}
 9 | 
10 | for i = 0, 1, 0.2 do
11 |     table.insert(ret, rom:lerp(i))
12 | end
13 | 
14 | print(ret)
15 | 


--------------------------------------------------------------------------------
/matlab/CatmullRom.m:
--------------------------------------------------------------------------------
  1 | classdef CatmullRom
  2 |     %CatmullRom 样条曲线
  3 |     %   给定一组控制点而得到一条曲线,曲线的大致形状由这些点控制
  4 |     
  5 |     properties
  6 |         %样条曲线经过的点
  7 |         points = []
  8 |         
  9 |         %到每一个点的曲线长度
 10 |         distances = []
 11 |         %每一段函数曲线，导函数ds，dx
 12 |         funcs = []
 13 |         %是否环路
 14 |         bClose = 0
 15 |         %点的个数
 16 |         length = 0
 17 |         %用法,默认均速曲线运动
 18 |         usage = 0
 19 |     end
 20 |     
 21 |     methods
 22 |         % usage
 23 |         %    0: 均速曲线运动,s变化恒定
 24 |         %    1: 自定义缓动曲线,t变化恒定
 25 |         function obj = CatmullRom(pts,usage)
 26 |             if nargin > 1
 27 |                 obj.usage = usage;
 28 |             end
 29 |             %CatmullRom 构造此类的实例
 30 |             obj.points = pts;
 31 |             
 32 |             sz = size(pts);
 33 |             obj.length = sz(2);
 34 |             obj.funcs = cell(1,obj.length);
 35 |             obj.bClose = pts(1) == pts(obj.length);
 36 |             
 37 |             sumDistance = 0;
 38 |             for i = 1:obj.length-1
 39 |                 if i == 1
 40 |                     if obj.bClose
 41 |                         %是环路，第一个点 为 倒数第二个点
 42 |                         p0 = pts(obj.length - 1);
 43 |                     else
 44 |                         %第一个点和第二个点在同一位置
 45 |                         p0 = pts(1);
 46 |                     end
 47 |                 else
 48 |                     p0 = pts(i - 1);
 49 |                 end
 50 |                 p1 = pts(i);
 51 |                 p2 = pts(i+1);
 52 |                 if i+1 == obj.length
 53 |                     if obj.bClose
 54 |                         %是环路，最后一个点指向第二个点
 55 |                         p3 = pts(2);
 56 |                     else
 57 |                         p3 = pts(obj.length);
 58 |                     end
 59 |                 else
 60 |                     p3 = pts(i+2);
 61 |                 end
 62 |                 [obj.funcs{i}{1}, ds,dx] = CatmullRom.curve(p0,p1,p2,p3);
 63 |                 if obj.usage == 0
 64 |                     obj.funcs{i}{2} = ds;
 65 |                 else
 66 |                     obj.funcs{i}{2} = dx;
 67 |                 end
 68 |                 sumDistance = sumDistance + CatmullRom.gaussLegendre(obj.funcs{i}{2},0,1);
 69 |                 obj.distances(i) = sumDistance;
 70 |             end
 71 |         end
 72 |         %插值
 73 |         function ret = lerp(obj,t)
 74 |             %第一个和最后一个点直接返回
 75 |             if t == 0
 76 |                 ret = obj.points(1);
 77 |                 return
 78 |             elseif t == 1
 79 |                 ret = obj.points(obj.length);
 80 |                 return
 81 |             end
 82 |             %平均距离
 83 |             averDis = t*obj.distances(obj.length-1);
 84 |             for i = 1:obj.length-1
 85 |                 if averDis < obj.distances(i)
 86 |                     index = i;
 87 |                     if i == 1
 88 |                         preDis = 0;
 89 |                     else
 90 |                         preDis = obj.distances(i - 1);
 91 |                     end
 92 |                     beyond = averDis - preDis;
 93 |                     percent = beyond / (obj.distances(i) - preDis);
 94 |                     break;
 95 |                 end
 96 |             end
 97 |             
 98 |             %牛顿切线法求根
 99 |             a = percent;
100 |             %最多迭代6次
101 |             for i = 0:0.2:1
102 |                 actualLen = CatmullRom.gaussLegendre(obj.funcs{index}{2},0,a);
103 |                 b = a - (actualLen - beyond)/obj.funcs{index}{2}(a);
104 |                 if abs(a - b)<0.0001
105 |                     break
106 |                 end
107 |                 a = b;
108 |             end
109 |             percent = b ;
110 |             ret = obj.funcs{index}{1}(percent);
111 |         end
112 |     end
113 |     methods(Static)
114 |         function [rfx,rdx,rdxx] = curve(p0, p1, p2, p3)
115 |             %CatmullRom
116 |             %   基本样条线插值算法
117 |             % 弹性
118 |             s =0.5;
119 |             %计算三次样条线函数系数
120 |             b1 = -s* p0 + (2-s)*p1+(s-2)*p2+s*p3;
121 |             b2 = 2*s*p0 +(s-3)*p1+(3-2*s)*p2+(-s)*p3;
122 |             b3 = -s* p0+s*p2;
123 |             b4 = p1;
124 |             
125 |             % 函数曲线
126 |             function ret =fx(x)
127 |                 ret = b1*x^3 + b2*x^2 + b3*x+b4;
128 |             end
129 |             rfx = @fx;
130 |             % 曲线长度变化率,用于匀速曲线运动
131 |             function ret =dx(x)
132 |                 der = 3*b1 *x^2 + 2*b2*x +b3;
133 |                 ret = sqrt(der.x^2+der.y^2+der.z^2);
134 |             end
135 |             rdx= @dx;
136 |             % 曲线x变化率,用于自定义缓动曲线
137 |             function ret =dxx(x)
138 |                 der = 3*b1 *x^2 + 2*b2*x +b3;
139 |                 ret = der.x;
140 |             end
141 |             rdxx = @dxx;
142 |         end
143 |         
144 |         function len = gaussLegendre(func,a,b)
145 |             %gaussLegendre 高斯—勒让德积分公式可以用较少节点数得到高精度的计算结果
146 |             % a     左区间
147 |             % b     右区间
148 |             % 3次系数
149 |             GauFactor = containers.Map({0.7745966692,0},{0.555555556,0.8888888889});
150 |             % 5次系数
151 |             %GauFactor = containers.Map({0.9061798459,0.5384693101,0},{0.2369268851,0.4786286705,0.5688888889});
152 |             %积分
153 |             GauSum = 0 ;
154 |             for key = keys(GauFactor)
155 |                 k = key{1};
156 |                 v = GauFactor(k);
157 |                 t = ((b-a)*k+a+b)/2;
158 |                 der = func(t);
159 |                 GauSum = GauSum+der*v;
160 |                 if k>0
161 |                     t = ((b-a)*(-k)+a+b)/2;
162 |                     der = func(t);
163 |                     GauSum = GauSum+der*v;
164 |                 end
165 |             end
166 |             len = GauSum*(b-a)/2;
167 |         end
168 |     end
169 | end
170 | 
171 | 


--------------------------------------------------------------------------------
/matlab/Vec3.m:
--------------------------------------------------------------------------------
 1 | classdef Vec3
 2 |     %Vec3 三维向量
 3 |     
 4 |     properties
 5 |         x = 0
 6 |         y = 0
 7 |         z = 0
 8 |     end
 9 |     
10 |     methods
11 |         function obj = Vec3(x,y,z)
12 |             %Vec3 构造此类的实例
13 |             if  nargin == 3
14 |                 obj.x = x;
15 |                 obj.y = y;
16 |                 obj.z = z;
17 |             elseif nargin == 1
18 |                 obj.x = x(1);
19 |                 obj.y = x(2);
20 |                 obj.z = x(3);
21 |             else
22 |                 obj.x = 0;
23 |                 obj.y = 0;
24 |                 obj.z = 0;
25 |             end
26 |         end
27 |         
28 |         function b = eq(obj,other)
29 |             %eq 重载 == 运算符
30 |             b = obj.x == other.x && obj.y == other.y && obj.z == other.z;
31 |         end
32 |         function b = minus(obj,other)
33 |             %minus 重载 - 运算符
34 |             b = Vec3(obj.x - other.x,obj.y - other.y,obj.z - other.z);
35 |         end
36 |         function b = plus(obj,other)
37 |             %plus 重载 + 运算符
38 |             if isa(other, 'double')
39 |                 b = Vec3(obj.x + other,obj.y + other,obj.z + other);
40 |             elseif isa(obj, 'double')
41 |                 b = Vec3(obj + other.x,obj + other.y,obj + other.z);
42 |             else
43 |                 b = Vec3(obj.x + other.x,obj.y + other.y,obj.z + other.z);
44 |             end
45 |         end
46 |         function b = mtimes(obj,other)
47 |             %mtimes 重载 * 运算符
48 |             if isa(other, 'double')
49 |                 b = Vec3(obj.x * other,obj.y * other,obj.z * other);
50 |             elseif isa(obj, 'double')
51 |                 b = Vec3(obj * other.x,obj * other.y,obj * other.z);
52 |             elseif isa(other, 'Vec3')
53 |                 b = Vec3(obj.x * other.x,obj.y * other.y,obj.z * other.z);
54 |             end
55 |         end
56 |         function b = mpower(obj,n)
57 |             %mpower 重载 ^ 运算符
58 |             b = Vec3(power(obj.x,n),power(obj.y,n),power(obj.z,n));
59 |         end
60 |     end
61 | end
62 | 
63 | 


--------------------------------------------------------------------------------
/matlab/test.m:
--------------------------------------------------------------------------------
 1 | %样条曲线经过的点
 2 | points=[Vec3(0.1,0.4,0),Vec3(0.5,0.4,0),Vec3(0.7,0.1,0),Vec3(0.9,0.9,0),Vec3(0.1,0.4,0)];
 3 | 
 4 | rom = CatmullRom(points);
 5 | ret = repmat(Vec3(),[0 0]);
 6 | 
 7 | for i = 0:0.02:1
 8 |     sz = size(ret);
 9 |     ret(sz(2)+1) = rom.lerp(i);
10 | end
11 | hold off
12 | plot([ret.x],[ret.y],'r.')
13 | 
14 | %坐标轴要固定，以便观察距离
15 | axis([0 1 0 1]);
16 | 


--------------------------------------------------------------------------------