1 /* Copyright (C) 2017 Wildfire Games.
2 * This file is part of 0 A.D.
3 *
4 * 0 A.D. is free software: you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation, either version 2 of the License, or
7 * (at your option) any later version.
8 *
9 * 0 A.D. is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
13 *
14 * You should have received a copy of the GNU General Public License
15 * along with 0 A.D. If not, see <http://www.gnu.org/licenses/>.
16 */
17
18 #include "precompiled.h"
19
20 #include <algorithm>
21
22 #include "NUSpline.h"
23 #include "Matrix3D.h"
24
25 //Note: column major order! Each set of 4 constitutes a column.
26 CMatrix3D HermiteSpline(2.f, -3.f, 0.f, 1.f,
27 -2.f, 3.f, 0.f, 0.f,
28 1.f, -2.f, 1.f, 0.f,
29 1.f, -1.f, 0.f, 0.f); // Matrix H in article
30
31
32 // cubic curve defined by 2 positions and 2 velocities
GetPositionOnCubic(const CVector3D & startPos,const CVector3D & startVel,const CVector3D & endPos,const CVector3D & endVel,float time)33 CVector3D GetPositionOnCubic(const CVector3D& startPos, const CVector3D& startVel, const CVector3D& endPos, const CVector3D& endVel, float time)
34 {
35 CMatrix3D m(startPos.X, endPos.X, startVel.X, endVel.X,
36 startPos.Y, endPos.Y, startVel.Y, endVel.Y,
37 startPos.Z, endPos.Z, startVel.Z, endVel.Z,
38 0.0f, 0.0f, 0.0f, 1.0f);
39
40 m = m * HermiteSpline; // multiply by the mixer
41
42 CVector3D TimeVector(time*time*time, time*time, time);
43 CVector3D Result;
44 m.Transform(TimeVector, Result);
45 return Result;
46 }
47
48 /*********************************** R N S **************************************************/
49
RNSpline()50 RNSpline::RNSpline()
51 : NodeCount(0)
52 {
53 }
54
55 RNSpline::~RNSpline() = default;
56
57 // adds node and updates segment length
AddNode(const CFixedVector3D & pos)58 void RNSpline::AddNode(const CFixedVector3D& pos)
59 {
60 if (NodeCount >= MAX_SPLINE_NODES)
61 return;
62 if (NodeCount == 0)
63 MaxDistance = fixed::Zero();
64 else
65 {
66 Node[NodeCount-1].Distance = (Node[NodeCount-1].Position - pos).Length();
67 MaxDistance += Node[NodeCount-1].Distance;
68 }
69 SplineData temp;
70 temp.Position = pos;
71 Node.push_back(temp);
72 ++NodeCount;
73 }
74
75
76 // called after all nodes added. This function calculates the node velocities
BuildSpline()77 void RNSpline::BuildSpline()
78 {
79 if (NodeCount == 2)
80 {
81 Node[0].Velocity = GetStartVelocity(0);
82 Node[NodeCount-1].Velocity = GetEndVelocity(NodeCount-1);
83 return;
84 }
85 else if (NodeCount < 2)
86 return;
87
88 for (int i = 1; i < NodeCount-1; ++i)
89 {
90 CVector3D Next = Node[i+1].Position - Node[i].Position;
91 CVector3D Previous = Node[i-1].Position - Node[i].Position;
92 Next.Normalize();
93 Previous.Normalize();
94
95 // split the angle (figure 4)
96 Node[i].Velocity = Next - Previous;
97 Node[i].Velocity.Normalize();
98 }
99 // calculate start and end velocities
100 Node[0].Velocity = GetStartVelocity(0);
101 Node[NodeCount-1].Velocity = GetEndVelocity(NodeCount-1);
102 }
103
104 // spline access function. time is 0 -> 1
GetPosition(float time) const105 CVector3D RNSpline::GetPosition(float time) const
106 {
107 if (NodeCount < 2)
108 return CVector3D(0.0f, 0.0f, 0.0f);
109 if (time < 0.0f)
110 time = 0.0f;
111 if (time > 1.0f)
112 time = 1.0f;
113 float Distance = time * MaxDistance.ToFloat();
114 float CurrentDistance = 0.f;
115 int i = 0;
116
117 // Find which node we're on
118 while (CurrentDistance + Node[i].Distance.ToFloat() < Distance && i < NodeCount - 2)
119 {
120 CurrentDistance += Node[i].Distance.ToFloat();
121 ++i;
122 }
123 ENSURE(i < NodeCount - 1);
124 float t = Distance - CurrentDistance;
125 // TODO: reimplement CVector3D comparator (float comparing is bad without EPS)
126 if (Node[i].Position == Node[i+1].Position || Node[i].Distance.ToFloat() < 1e-7) // distance too small or zero
127 {
128 return Node[i+1].Position;
129 }
130 t /= Node[i].Distance.ToFloat(); // scale t in range 0 - 1
131 CVector3D startVel = Node[i].Velocity * Node[i].Distance.ToFloat();
132 CVector3D endVel = Node[i+1].Velocity * Node[i].Distance.ToFloat();
133 return GetPositionOnCubic(Node[i].Position, startVel,
134 Node[i+1].Position, endVel, t);
135 }
136
GetAllNodes() const137 const std::vector<SplineData>& RNSpline::GetAllNodes() const
138 {
139 return Node;
140 }
141
142 // internal. Based on Equation 14
GetStartVelocity(int index)143 CVector3D RNSpline::GetStartVelocity(int index)
144 {
145 if (index >= NodeCount - 1 || index < 0)
146 return CVector3D(0.0f, 0.0f, 0.0f);
147 CVector3D temp = CVector3D(Node[index+1].Position - Node[index].Position) * 3.0f * (1.0f / Node[index].Distance.ToFloat());
148 return (temp - Node[index+1].Velocity)*0.5f;
149 }
150
151 // internal. Based on Equation 15
GetEndVelocity(int index)152 CVector3D RNSpline::GetEndVelocity(int index)
153 {
154 if (index >= NodeCount || index < 1)
155 return CVector3D(0.0f, 0.0f, 0.0f);
156 CVector3D temp = CVector3D(Node[index].Position - Node[index-1].Position) * 3.0f * (1.0f / Node[index-1].Distance.ToFloat());
157 return (temp - Node[index-1].Velocity) * 0.5f;
158 }
159
160 /*********************************** S N S **************************************************/
161
162 SNSpline::~SNSpline() = default;
163
BuildSpline()164 void SNSpline::BuildSpline()
165 {
166 RNSpline::BuildSpline();
167 for (int i = 0; i < 3; ++i)
168 Smooth();
169 }
170
171 // smoothing filter.
Smooth()172 void SNSpline::Smooth()
173 {
174 if (NodeCount < 3)
175 return;
176
177 CVector3D newVel;
178 CVector3D oldVel = GetStartVelocity(0);
179 for (int i = 1; i < NodeCount-1; ++i)
180 {
181 // Equation 12
182 newVel = GetEndVelocity(i) * Node[i].Distance.ToFloat() + GetStartVelocity(i) * Node[i-1].Distance.ToFloat();
183 newVel = newVel * (1 / (Node[i-1].Distance + Node[i].Distance).ToFloat());
184 Node[i-1].Velocity = oldVel;
185 oldVel = newVel;
186 }
187 Node[NodeCount-1].Velocity = GetEndVelocity(NodeCount-1);
188 Node[NodeCount-2].Velocity = oldVel;
189 }
190
191 /*********************************** T N S **************************************************/
192
193 TNSpline::~TNSpline() = default;
194
195 // as with RNSpline but use timePeriod in place of actual node spacing
196 // ie time period is time from last node to this node
AddNode(const CFixedVector3D & pos,const CFixedVector3D & rotation,fixed timePeriod)197 void TNSpline::AddNode(const CFixedVector3D& pos, const CFixedVector3D& rotation, fixed timePeriod)
198 {
199 if (NodeCount >= MAX_SPLINE_NODES)
200 return;
201
202 if (NodeCount == 0)
203 MaxDistance = fixed::Zero();
204 else
205 {
206 Node[NodeCount-1].Distance = timePeriod;
207 MaxDistance += Node[NodeCount-1].Distance;
208 }
209
210 SplineData temp;
211 temp.Position = pos;
212
213 //make sure we don't end up using undefined numbers...
214 temp.Distance = fixed::Zero();
215 temp.Velocity = CVector3D(0.0f, 0.0f, 0.0f);
216 temp.Rotation = rotation;
217 Node.push_back(temp);
218 ++NodeCount;
219 }
220
221 //Inserts node before position
InsertNode(const int index,const CFixedVector3D & pos,const CFixedVector3D & UNUSED (rotation),fixed timePeriod)222 void TNSpline::InsertNode(const int index, const CFixedVector3D& pos, const CFixedVector3D& UNUSED(rotation), fixed timePeriod)
223 {
224 if (NodeCount >= MAX_SPLINE_NODES || index < 0 || index > NodeCount)
225 return;
226
227 if (NodeCount == 0)
228 MaxDistance = fixed::Zero();
229 else
230 MaxDistance += timePeriod;
231
232 SplineData temp;
233 temp.Position = pos;
234 temp.Distance = timePeriod;
235 Node.insert(Node.begin() + index, temp);
236 if (index > 0)
237 std::swap(Node[index].Distance, Node[index - 1].Distance);
238 ++NodeCount;
239 }
240
241 //Removes node at index
RemoveNode(const int index)242 void TNSpline::RemoveNode(const int index)
243 {
244 if (NodeCount == 0 || index > NodeCount - 1)
245 return;
246
247 MaxDistance -= Node[index].Distance;
248 Node.erase(Node.begin() + index);
249 --NodeCount;
250 }
251
UpdateNodeTime(const int index,fixed time)252 void TNSpline::UpdateNodeTime(const int index, fixed time)
253 {
254 if (NodeCount == 0 || index > NodeCount - 1)
255 return;
256
257 Node[index].Distance = time;
258 }
259
UpdateNodePos(const int index,const CFixedVector3D & pos)260 void TNSpline::UpdateNodePos(const int index, const CFixedVector3D& pos)
261 {
262 if (NodeCount == 0 || index > NodeCount - 1)
263 return;
264
265 Node[index].Position = pos;
266 }
267
BuildSpline()268 void TNSpline::BuildSpline()
269 {
270 RNSpline::BuildSpline();
271 for (int i = 0; i < 3; ++i)
272 Smooth();
273 }
274
Smooth()275 void TNSpline::Smooth()
276 {
277 for (int i = 0; i < 3; ++i)
278 {
279 SNSpline::Smooth();
280 Constrain();
281 }
282 }
283
Constrain()284 void TNSpline::Constrain()
285 {
286 if (NodeCount < 3)
287 return;
288
289 for (int i = 1; i < NodeCount-1; ++i)
290 {
291 // Equation 13
292 float r0 = (Node[i].Position - Node[i - 1].Position).Length().ToFloat() / Node[i-1].Distance.ToFloat();
293 float r1 = (Node[i+1].Position - Node[i].Position).Length().ToFloat() / Node[i].Distance.ToFloat();
294 Node[i].Velocity *= 4.0f*r0*r1 / ((r0 + r1)*(r0 + r1));
295 }
296 }
297
298