LCOV - code coverage report
Current view: top level - source/maths - NUSpline.cpp (source / functions) Hit Total Coverage
Test: 0 A.D. test coverage report Lines: 49 151 32.5 %
Date: 2023-01-19 00:18:29 Functions: 12 20 60.0 %

          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             : 

Generated by: LCOV version 1.13