mirror of
https://gitea.wildfiregames.com/0ad/0ad
synced 2026-08-15 14:43:32 -07:00
53d1ae00f1
The last node needs to have distance "0" to next node. Do the inverse of when adding nodes. Also add some basic tests covering this bug. Fixes: #4659 Signed-off-by: Ralph Sennhauser <ralph.sennhauser@gmail.com>
302 lines
8.3 KiB
C++
302 lines
8.3 KiB
C++
/* Copyright (C) 2026 Wildfire Games.
|
|
* This file is part of 0 A.D.
|
|
*
|
|
* 0 A.D. is free software: you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation, either version 2 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* 0 A.D. is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with 0 A.D. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include "precompiled.h"
|
|
|
|
#include "NUSpline.h"
|
|
|
|
#include "lib/debug.h"
|
|
#include "maths/FixedVector3D.h"
|
|
#include "maths/Matrix3D.h"
|
|
#include "maths/Vector3D.h"
|
|
|
|
#include <utility>
|
|
|
|
|
|
//Note: column major order! Each set of 4 constitutes a column.
|
|
CMatrix3D HermiteSpline(2.f, -3.f, 0.f, 1.f,
|
|
-2.f, 3.f, 0.f, 0.f,
|
|
1.f, -2.f, 1.f, 0.f,
|
|
1.f, -1.f, 0.f, 0.f); // Matrix H in article
|
|
|
|
|
|
// cubic curve defined by 2 positions and 2 velocities
|
|
CVector3D GetPositionOnCubic(const CVector3D& startPos, const CVector3D& startVel, const CVector3D& endPos, const CVector3D& endVel, float time)
|
|
{
|
|
CMatrix3D m(startPos.X, endPos.X, startVel.X, endVel.X,
|
|
startPos.Y, endPos.Y, startVel.Y, endVel.Y,
|
|
startPos.Z, endPos.Z, startVel.Z, endVel.Z,
|
|
0.0f, 0.0f, 0.0f, 1.0f);
|
|
|
|
m = m * HermiteSpline; // multiply by the mixer
|
|
|
|
CVector3D TimeVector(time*time*time, time*time, time);
|
|
CVector3D Result;
|
|
m.Transform(TimeVector, Result);
|
|
return Result;
|
|
}
|
|
|
|
/*********************************** R N S **************************************************/
|
|
|
|
RNSpline::RNSpline() = default;
|
|
|
|
RNSpline::~RNSpline() = default;
|
|
|
|
// adds node and updates segment length
|
|
void RNSpline::AddNode(const CFixedVector3D& pos)
|
|
{
|
|
if (m_NodeCount >= MAX_SPLINE_NODES)
|
|
return;
|
|
if (m_NodeCount == 0)
|
|
m_MaxDistance = fixed::Zero();
|
|
else
|
|
{
|
|
m_Nodes[m_NodeCount-1].Distance = (m_Nodes[m_NodeCount-1].Position - pos).Length();
|
|
m_MaxDistance += m_Nodes[m_NodeCount-1].Distance;
|
|
}
|
|
SplineData temp;
|
|
temp.Position = pos;
|
|
m_Nodes.push_back(temp);
|
|
++m_NodeCount;
|
|
}
|
|
|
|
|
|
// called after all nodes added. This function calculates the node velocities
|
|
void RNSpline::BuildSpline()
|
|
{
|
|
if (m_NodeCount == 2)
|
|
{
|
|
m_Nodes[0].Velocity = GetStartVelocity(0);
|
|
m_Nodes[m_NodeCount-1].Velocity = GetEndVelocity(m_NodeCount-1);
|
|
return;
|
|
}
|
|
else if (m_NodeCount < 2)
|
|
return;
|
|
|
|
for (int i = 1; i < m_NodeCount-1; ++i)
|
|
{
|
|
CVector3D Next = m_Nodes[i+1].Position - m_Nodes[i].Position;
|
|
CVector3D Previous = m_Nodes[i-1].Position - m_Nodes[i].Position;
|
|
Next.Normalize();
|
|
Previous.Normalize();
|
|
|
|
// split the angle (figure 4)
|
|
m_Nodes[i].Velocity = Next - Previous;
|
|
m_Nodes[i].Velocity.Normalize();
|
|
}
|
|
// calculate start and end velocities
|
|
m_Nodes[0].Velocity = GetStartVelocity(0);
|
|
m_Nodes[m_NodeCount-1].Velocity = GetEndVelocity(m_NodeCount-1);
|
|
}
|
|
|
|
// spline access function. time is 0 -> 1
|
|
CVector3D RNSpline::GetPosition(float time) const
|
|
{
|
|
if (m_NodeCount < 2)
|
|
return CVector3D(0.0f, 0.0f, 0.0f);
|
|
if (time < 0.0f)
|
|
time = 0.0f;
|
|
if (time > 1.0f)
|
|
time = 1.0f;
|
|
float Distance = time * m_MaxDistance.ToFloat();
|
|
float CurrentDistance = 0.f;
|
|
int i = 0;
|
|
|
|
// Find which node we're on
|
|
while (CurrentDistance + m_Nodes[i].Distance.ToFloat() < Distance && i < m_NodeCount - 2)
|
|
{
|
|
CurrentDistance += m_Nodes[i].Distance.ToFloat();
|
|
++i;
|
|
}
|
|
ENSURE(i < m_NodeCount - 1);
|
|
float t = Distance - CurrentDistance;
|
|
// TODO: reimplement CVector3D comparator (float comparing is bad without EPS)
|
|
if (m_Nodes[i].Position == m_Nodes[i+1].Position || m_Nodes[i].Distance.ToFloat() < 1e-7) // distance too small or zero
|
|
{
|
|
return m_Nodes[i+1].Position;
|
|
}
|
|
t /= m_Nodes[i].Distance.ToFloat(); // scale t in range 0 - 1
|
|
CVector3D startVel = m_Nodes[i].Velocity * m_Nodes[i].Distance.ToFloat();
|
|
CVector3D endVel = m_Nodes[i+1].Velocity * m_Nodes[i].Distance.ToFloat();
|
|
return GetPositionOnCubic(m_Nodes[i].Position, startVel,
|
|
m_Nodes[i+1].Position, endVel, t);
|
|
}
|
|
|
|
const std::vector<SplineData>& RNSpline::GetAllNodes() const
|
|
{
|
|
return m_Nodes;
|
|
}
|
|
|
|
// internal. Based on Equation 14
|
|
CVector3D RNSpline::GetStartVelocity(int index)
|
|
{
|
|
if (index >= m_NodeCount - 1 || index < 0)
|
|
return CVector3D(0.0f, 0.0f, 0.0f);
|
|
CVector3D temp = CVector3D(m_Nodes[index+1].Position - m_Nodes[index].Position) * 3.0f * (1.0f / m_Nodes[index].Distance.ToFloat());
|
|
return (temp - m_Nodes[index+1].Velocity)*0.5f;
|
|
}
|
|
|
|
// internal. Based on Equation 15
|
|
CVector3D RNSpline::GetEndVelocity(int index)
|
|
{
|
|
if (index >= m_NodeCount || index < 1)
|
|
return CVector3D(0.0f, 0.0f, 0.0f);
|
|
CVector3D temp = CVector3D(m_Nodes[index].Position - m_Nodes[index-1].Position) * 3.0f * (1.0f / m_Nodes[index-1].Distance.ToFloat());
|
|
return (temp - m_Nodes[index-1].Velocity) * 0.5f;
|
|
}
|
|
|
|
/*********************************** S N S **************************************************/
|
|
|
|
SNSpline::~SNSpline() = default;
|
|
|
|
void SNSpline::BuildSpline()
|
|
{
|
|
RNSpline::BuildSpline();
|
|
for (int i = 0; i < 3; ++i)
|
|
Smooth();
|
|
}
|
|
|
|
// smoothing filter.
|
|
void SNSpline::Smooth()
|
|
{
|
|
if (m_NodeCount < 3)
|
|
return;
|
|
|
|
CVector3D newVel;
|
|
CVector3D oldVel = GetStartVelocity(0);
|
|
for (int i = 1; i < m_NodeCount-1; ++i)
|
|
{
|
|
// Equation 12
|
|
newVel = GetEndVelocity(i) * m_Nodes[i].Distance.ToFloat() + GetStartVelocity(i) * m_Nodes[i-1].Distance.ToFloat();
|
|
newVel = newVel * (1 / (m_Nodes[i-1].Distance + m_Nodes[i].Distance).ToFloat());
|
|
m_Nodes[i-1].Velocity = oldVel;
|
|
oldVel = newVel;
|
|
}
|
|
m_Nodes[m_NodeCount-1].Velocity = GetEndVelocity(m_NodeCount-1);
|
|
m_Nodes[m_NodeCount-2].Velocity = oldVel;
|
|
}
|
|
|
|
/*********************************** T N S **************************************************/
|
|
|
|
TNSpline::~TNSpline() = default;
|
|
|
|
// as with RNSpline but use timePeriod in place of actual node spacing
|
|
// ie time period is time from last node to this node
|
|
void TNSpline::AddNode(const CFixedVector3D& pos, const CFixedVector3D& rotation, fixed timePeriod)
|
|
{
|
|
if (m_NodeCount >= MAX_SPLINE_NODES)
|
|
return;
|
|
|
|
if (m_NodeCount == 0)
|
|
m_MaxDistance = fixed::Zero();
|
|
else
|
|
{
|
|
m_Nodes[m_NodeCount-1].Distance = timePeriod;
|
|
m_MaxDistance += m_Nodes[m_NodeCount-1].Distance;
|
|
}
|
|
|
|
SplineData temp;
|
|
temp.Position = pos;
|
|
|
|
//make sure we don't end up using undefined numbers...
|
|
temp.Distance = fixed::Zero();
|
|
temp.Velocity = CVector3D(0.0f, 0.0f, 0.0f);
|
|
temp.Rotation = rotation;
|
|
m_Nodes.push_back(temp);
|
|
++m_NodeCount;
|
|
}
|
|
|
|
//Inserts node before position
|
|
void TNSpline::InsertNode(const int index, const CFixedVector3D& pos, const CFixedVector3D& /*rotation*/,
|
|
fixed timePeriod)
|
|
{
|
|
if (m_NodeCount >= MAX_SPLINE_NODES || index < 0 || index > m_NodeCount)
|
|
return;
|
|
|
|
if (m_NodeCount == 0)
|
|
m_MaxDistance = fixed::Zero();
|
|
else
|
|
m_MaxDistance += timePeriod;
|
|
|
|
SplineData temp;
|
|
temp.Position = pos;
|
|
temp.Distance = timePeriod;
|
|
m_Nodes.insert(m_Nodes.begin() + index, temp);
|
|
if (index > 0)
|
|
std::swap(m_Nodes[index].Distance, m_Nodes[index - 1].Distance);
|
|
++m_NodeCount;
|
|
}
|
|
|
|
//Removes node at index
|
|
void TNSpline::RemoveNode(const int index)
|
|
{
|
|
if (m_NodeCount == 0 || index > m_NodeCount - 1)
|
|
return;
|
|
|
|
if (index > 0)
|
|
std::swap(m_Nodes[index].Distance, m_Nodes[index - 1].Distance);
|
|
m_MaxDistance -= m_Nodes[index].Distance;
|
|
m_Nodes.erase(m_Nodes.begin() + index);
|
|
--m_NodeCount;
|
|
}
|
|
|
|
void TNSpline::UpdateNodeTime(const int index, fixed time)
|
|
{
|
|
if (m_NodeCount == 0 || index > m_NodeCount - 1)
|
|
return;
|
|
|
|
m_Nodes[index].Distance = time;
|
|
}
|
|
|
|
void TNSpline::UpdateNodePos(const int index, const CFixedVector3D& pos)
|
|
{
|
|
if (m_NodeCount == 0 || index > m_NodeCount - 1)
|
|
return;
|
|
|
|
m_Nodes[index].Position = pos;
|
|
}
|
|
|
|
void TNSpline::BuildSpline()
|
|
{
|
|
RNSpline::BuildSpline();
|
|
for (int i = 0; i < 3; ++i)
|
|
Smooth();
|
|
}
|
|
|
|
void TNSpline::Smooth()
|
|
{
|
|
for (int i = 0; i < 3; ++i)
|
|
{
|
|
SNSpline::Smooth();
|
|
Constrain();
|
|
}
|
|
}
|
|
|
|
void TNSpline::Constrain()
|
|
{
|
|
if (m_NodeCount < 3)
|
|
return;
|
|
|
|
for (int i = 1; i < m_NodeCount-1; ++i)
|
|
{
|
|
// Equation 13
|
|
float r0 = (m_Nodes[i].Position - m_Nodes[i - 1].Position).Length().ToFloat() / m_Nodes[i-1].Distance.ToFloat();
|
|
float r1 = (m_Nodes[i+1].Position - m_Nodes[i].Position).Length().ToFloat() / m_Nodes[i].Distance.ToFloat();
|
|
m_Nodes[i].Velocity *= 4.0f*r0*r1 / ((r0 + r1)*(r0 + r1));
|
|
}
|
|
}
|
|
|