source/maths/NUSpline.cpp

   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
  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
  50 RNSpline::RNSpline()
  51         : NodeCount(0)
  52 {
  53 }
  54
  55 RNSpline::~RNSpline() = default;
  56
  57 // adds node and updates segment length
  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
  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
 105 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
 137 const std::vector<SplineData>& RNSpline::GetAllNodes() const
 138 {
 139         return Node;
 140 }
 141
 142 // internal. Based on Equation 14
 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
 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
 164 void SNSpline::BuildSpline()
 165 {
 166         RNSpline::BuildSpline();
 167         for (int i = 0; i < 3; ++i)
 168                 Smooth();
 169 }
 170
 171 // smoothing filter.
 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
 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
 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
 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
 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
 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
 268 void TNSpline::BuildSpline()
 269 {
 270         RNSpline::BuildSpline();
 271         for (int i = 0; i < 3; ++i)
 272                 Smooth();
 273 }
 274
 275 void TNSpline::Smooth()
 276 {
 277         for (int i = 0; i < 3; ++i)
 278         {
 279                 SNSpline::Smooth();
 280                 Constrain();
 281         }
 282 }
 283
 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