Line data Source code
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 : 1.f, -1.f, 0.f, 0.f); // Matrix H in article
30 :
31 :
32 : // cubic curve defined by 2 positions and 2 velocities
33 0 : CVector3D GetPositionOnCubic(const CVector3D& startPos, const CVector3D& startVel, const CVector3D& endPos, const CVector3D& endVel, float time)
34 : {
35 0 : CMatrix3D m(startPos.X, endPos.X, startVel.X, endVel.X,
36 0 : startPos.Y, endPos.Y, startVel.Y, endVel.Y,
37 0 : startPos.Z, endPos.Z, startVel.Z, endVel.Z,
38 0 : 0.0f, 0.0f, 0.0f, 1.0f);
39 :
40 0 : m = m * HermiteSpline; // multiply by the mixer
41 :
42 0 : CVector3D TimeVector(time*time*time, time*time, time);
43 0 : CVector3D Result;
44 0 : m.Transform(TimeVector, Result);
45 0 : return Result;
46 : }
47 :
48 : /*********************************** R N S **************************************************/
49 :
50 8 : RNSpline::RNSpline()
51 8 : : NodeCount(0)
52 : {
53 8 : }
54 :
55 : RNSpline::~RNSpline() = default;
56 :
57 : // adds node and updates segment length
58 0 : void RNSpline::AddNode(const CFixedVector3D& pos)
59 : {
60 0 : if (NodeCount >= MAX_SPLINE_NODES)
61 0 : return;
62 0 : if (NodeCount == 0)
63 0 : MaxDistance = fixed::Zero();
64 : else
65 : {
66 0 : Node[NodeCount-1].Distance = (Node[NodeCount-1].Position - pos).Length();
67 0 : MaxDistance += Node[NodeCount-1].Distance;
68 : }
69 0 : SplineData temp;
70 0 : temp.Position = pos;
71 0 : Node.push_back(temp);
72 0 : ++NodeCount;
73 : }
74 :
75 :
76 : // called after all nodes added. This function calculates the node velocities
77 4 : void RNSpline::BuildSpline()
78 : {
79 4 : if (NodeCount == 2)
80 : {
81 4 : Node[0].Velocity = GetStartVelocity(0);
82 4 : Node[NodeCount-1].Velocity = GetEndVelocity(NodeCount-1);
83 4 : return;
84 : }
85 0 : else if (NodeCount < 2)
86 0 : return;
87 :
88 0 : for (int i = 1; i < NodeCount-1; ++i)
89 : {
90 0 : CVector3D Next = Node[i+1].Position - Node[i].Position;
91 0 : CVector3D Previous = Node[i-1].Position - Node[i].Position;
92 0 : Next.Normalize();
93 0 : Previous.Normalize();
94 :
95 : // split the angle (figure 4)
96 0 : Node[i].Velocity = Next - Previous;
97 0 : Node[i].Velocity.Normalize();
98 : }
99 : // calculate start and end velocities
100 0 : Node[0].Velocity = GetStartVelocity(0);
101 0 : Node[NodeCount-1].Velocity = GetEndVelocity(NodeCount-1);
102 : }
103 :
104 : // spline access function. time is 0 -> 1
105 0 : CVector3D RNSpline::GetPosition(float time) const
106 : {
107 0 : if (NodeCount < 2)
108 0 : return CVector3D(0.0f, 0.0f, 0.0f);
109 0 : if (time < 0.0f)
110 0 : time = 0.0f;
111 0 : if (time > 1.0f)
112 0 : time = 1.0f;
113 0 : float Distance = time * MaxDistance.ToFloat();
114 0 : float CurrentDistance = 0.f;
115 0 : int i = 0;
116 :
117 : // Find which node we're on
118 0 : while (CurrentDistance + Node[i].Distance.ToFloat() < Distance && i < NodeCount - 2)
119 : {
120 0 : CurrentDistance += Node[i].Distance.ToFloat();
121 0 : ++i;
122 : }
123 0 : ENSURE(i < NodeCount - 1);
124 0 : float t = Distance - CurrentDistance;
125 : // TODO: reimplement CVector3D comparator (float comparing is bad without EPS)
126 0 : if (Node[i].Position == Node[i+1].Position || Node[i].Distance.ToFloat() < 1e-7) // distance too small or zero
127 : {
128 0 : return Node[i+1].Position;
129 : }
130 0 : t /= Node[i].Distance.ToFloat(); // scale t in range 0 - 1
131 0 : CVector3D startVel = Node[i].Velocity * Node[i].Distance.ToFloat();
132 0 : CVector3D endVel = Node[i+1].Velocity * Node[i].Distance.ToFloat();
133 0 : return GetPositionOnCubic(Node[i].Position, startVel,
134 0 : Node[i+1].Position, endVel, t);
135 : }
136 :
137 2 : const std::vector<SplineData>& RNSpline::GetAllNodes() const
138 : {
139 2 : return Node;
140 : }
141 :
142 : // internal. Based on Equation 14
143 4 : CVector3D RNSpline::GetStartVelocity(int index)
144 : {
145 4 : if (index >= NodeCount - 1 || index < 0)
146 0 : return CVector3D(0.0f, 0.0f, 0.0f);
147 4 : CVector3D temp = CVector3D(Node[index+1].Position - Node[index].Position) * 3.0f * (1.0f / Node[index].Distance.ToFloat());
148 4 : return (temp - Node[index+1].Velocity)*0.5f;
149 : }
150 :
151 : // internal. Based on Equation 15
152 4 : CVector3D RNSpline::GetEndVelocity(int index)
153 : {
154 4 : if (index >= NodeCount || index < 1)
155 0 : return CVector3D(0.0f, 0.0f, 0.0f);
156 4 : CVector3D temp = CVector3D(Node[index].Position - Node[index-1].Position) * 3.0f * (1.0f / Node[index-1].Distance.ToFloat());
157 4 : return (temp - Node[index-1].Velocity) * 0.5f;
158 : }
159 :
160 : /*********************************** S N S **************************************************/
161 :
162 : SNSpline::~SNSpline() = default;
163 :
164 0 : void SNSpline::BuildSpline()
165 : {
166 0 : RNSpline::BuildSpline();
167 0 : for (int i = 0; i < 3; ++i)
168 0 : Smooth();
169 0 : }
170 :
171 : // smoothing filter.
172 36 : void SNSpline::Smooth()
173 : {
174 36 : if (NodeCount < 3)
175 36 : return;
176 :
177 0 : CVector3D newVel;
178 0 : CVector3D oldVel = GetStartVelocity(0);
179 0 : for (int i = 1; i < NodeCount-1; ++i)
180 : {
181 : // Equation 12
182 0 : newVel = GetEndVelocity(i) * Node[i].Distance.ToFloat() + GetStartVelocity(i) * Node[i-1].Distance.ToFloat();
183 0 : newVel = newVel * (1 / (Node[i-1].Distance + Node[i].Distance).ToFloat());
184 0 : Node[i-1].Velocity = oldVel;
185 0 : oldVel = newVel;
186 : }
187 0 : Node[NodeCount-1].Velocity = GetEndVelocity(NodeCount-1);
188 0 : 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
197 8 : void TNSpline::AddNode(const CFixedVector3D& pos, const CFixedVector3D& rotation, fixed timePeriod)
198 : {
199 8 : if (NodeCount >= MAX_SPLINE_NODES)
200 0 : return;
201 :
202 8 : if (NodeCount == 0)
203 4 : MaxDistance = fixed::Zero();
204 : else
205 : {
206 4 : Node[NodeCount-1].Distance = timePeriod;
207 4 : MaxDistance += Node[NodeCount-1].Distance;
208 : }
209 :
210 8 : SplineData temp;
211 8 : temp.Position = pos;
212 :
213 : //make sure we don't end up using undefined numbers...
214 8 : temp.Distance = fixed::Zero();
215 8 : temp.Velocity = CVector3D(0.0f, 0.0f, 0.0f);
216 8 : temp.Rotation = rotation;
217 8 : Node.push_back(temp);
218 8 : ++NodeCount;
219 : }
220 :
221 : //Inserts node before position
222 0 : void TNSpline::InsertNode(const int index, const CFixedVector3D& pos, const CFixedVector3D& UNUSED(rotation), fixed timePeriod)
223 : {
224 0 : if (NodeCount >= MAX_SPLINE_NODES || index < 0 || index > NodeCount)
225 0 : return;
226 :
227 0 : if (NodeCount == 0)
228 0 : MaxDistance = fixed::Zero();
229 : else
230 0 : MaxDistance += timePeriod;
231 :
232 0 : SplineData temp;
233 0 : temp.Position = pos;
234 0 : temp.Distance = timePeriod;
235 0 : Node.insert(Node.begin() + index, temp);
236 0 : if (index > 0)
237 0 : std::swap(Node[index].Distance, Node[index - 1].Distance);
238 0 : ++NodeCount;
239 : }
240 :
241 : //Removes node at index
242 0 : void TNSpline::RemoveNode(const int index)
243 : {
244 0 : if (NodeCount == 0 || index > NodeCount - 1)
245 0 : return;
246 :
247 0 : MaxDistance -= Node[index].Distance;
248 0 : Node.erase(Node.begin() + index);
249 0 : --NodeCount;
250 : }
251 :
252 0 : void TNSpline::UpdateNodeTime(const int index, fixed time)
253 : {
254 0 : if (NodeCount == 0 || index > NodeCount - 1)
255 0 : return;
256 :
257 0 : Node[index].Distance = time;
258 : }
259 :
260 0 : void TNSpline::UpdateNodePos(const int index, const CFixedVector3D& pos)
261 : {
262 0 : if (NodeCount == 0 || index > NodeCount - 1)
263 0 : return;
264 :
265 0 : Node[index].Position = pos;
266 : }
267 :
268 4 : void TNSpline::BuildSpline()
269 : {
270 4 : RNSpline::BuildSpline();
271 16 : for (int i = 0; i < 3; ++i)
272 12 : Smooth();
273 4 : }
274 :
275 12 : void TNSpline::Smooth()
276 : {
277 48 : for (int i = 0; i < 3; ++i)
278 : {
279 36 : SNSpline::Smooth();
280 36 : Constrain();
281 : }
282 12 : }
283 :
284 36 : void TNSpline::Constrain()
285 : {
286 36 : if (NodeCount < 3)
287 36 : return;
288 :
289 0 : for (int i = 1; i < NodeCount-1; ++i)
290 : {
291 : // Equation 13
292 0 : float r0 = (Node[i].Position - Node[i - 1].Position).Length().ToFloat() / Node[i-1].Distance.ToFloat();
293 0 : float r1 = (Node[i+1].Position - Node[i].Position).Length().ToFloat() / Node[i].Distance.ToFloat();
294 0 : Node[i].Velocity *= 4.0f*r0*r1 / ((r0 + r1)*(r0 + r1));
295 : }
296 3 : }
297 :
|
