├── .gitignore ├── images ├── pick.gif ├── agent.gif ├── bounds.gif ├── points.gif ├── subdivide.gif └── self-intersect.gif ├── CMakeLists.txt ├── Makefile ├── LICENSE.md ├── README.md ├── extra ├── SplinesDraw.hpp └── SplinesDraw.cpp ├── RotSplines.hpp ├── SplinesTest.cpp ├── test-ref.txt ├── VLMini.hpp ├── RotSplines.cpp └── Splines.hpp /.gitignore: -------------------------------------------------------------------------------- 1 | splines_test 2 | test.txt 3 | 4 | -------------------------------------------------------------------------------- /images/pick.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/andrewwillmott/splines-lib/HEAD/images/pick.gif -------------------------------------------------------------------------------- /images/agent.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/andrewwillmott/splines-lib/HEAD/images/agent.gif -------------------------------------------------------------------------------- /images/bounds.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/andrewwillmott/splines-lib/HEAD/images/bounds.gif -------------------------------------------------------------------------------- /images/points.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/andrewwillmott/splines-lib/HEAD/images/points.gif -------------------------------------------------------------------------------- /images/subdivide.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/andrewwillmott/splines-lib/HEAD/images/subdivide.gif -------------------------------------------------------------------------------- /images/self-intersect.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/andrewwillmott/splines-lib/HEAD/images/self-intersect.gif -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4) 2 | project(splines-lib) 3 | 4 | file(GLOB SOURCES 5 | "*.h" 6 | "*.cpp") 7 | include_directories(.) 8 | add_library(splines-lib STATIC ${SOURCES}) 9 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | CXXFLAGS = --std=c++11 -Wall -O2 2 | 3 | SPLINE_LIB_INCLUDES=Splines.hpp RotSplines.hpp VLMini.hpp Makefile 4 | SPLINE_LIB_SOURCES=Splines.cpp RotSplines.cpp 5 | 6 | splines_test: $(SPLINE_LIB_INCLUDES) $(SPLINE_LIB_SOURCES) SplinesTest.cpp 7 | $(CXX) $(CXXFLAGS) -o $@ $(SPLINE_LIB_SOURCES) SplinesTest.cpp 8 | 9 | clean: 10 | $(RM) splines_test 11 | 12 | test: splines_test 13 | @./splines_test > test.txt 14 | @! (diff test.txt test-ref.txt) || echo "All good" 15 | -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | This is free and unencumbered software released into the public domain. 2 | 3 | Anyone is free to copy, modify, publish, use, compile, sell, or 4 | distribute this software, either in source code form or as a compiled 5 | binary, for any purpose, commercial or non-commercial, and by any 6 | means. 7 | 8 | In jurisdictions that recognize copyright laws, the author or authors 9 | of this software dedicate any and all copyright interest in the 10 | software to the public domain. We make this dedication for the benefit 11 | of the public at large and to the detriment of our heirs and 12 | successors. We intend this dedication to be an overt act of 13 | relinquishment in perpetuity of all present and future rights to this 14 | software under copyright law. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 17 | EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 18 | MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. 19 | IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR 20 | OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, 21 | ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR 22 | OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | For more information, please refer to 25 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | SplineLib 2 | ========= 3 | 4 | Library for manipulating 1D, 2D, and 3D splines. Functionality included: 5 | 6 | * Creation from Bezier, Hermite, and Catmull-Rom forms 7 | * Creation of an array of splines from an array of points and tension value, 8 | or Bezier hull points, or Hermite tangents. 9 | * Creation of arcs and circles 10 | * Evaluation of position, velocity, acceleration, curvature, torsion, and 11 | tangent frame 12 | * Length measurement 13 | * Finding bounds 14 | * Offsetting (e.g., for stroking) and reversing splines 15 | * Splitting and re-joining of single splines 16 | * Subdivision of runs of splines either evenly, for flatness, or to be linear in 17 | arc length 18 | * Conversion of runs of splines to line segments, e.g., for drawing 19 | * Finding the closest point on a run of splines 20 | * Finding where runs of splines intersect, or a run of splines self-intersects 21 | * Helpers for advancing a point along a spline at some given velocity 22 | * Support for monotonic splines, which don't extrapolate outside the source 23 | points 24 | * Support for rotational splines in RotSplines.*, comprising of angle 25 | interpolation in 2D, and quaternion interpolation in 3D. Includes code for 26 | evaluation of rotational velocity and acceleration for both. 27 | 28 | There is an [interactive 29 | demo](https://andrewwillmott.github.io/app/DemoApp.html?test=Splines) of some of 30 | this functionality. 31 | 32 | As an example of a use case, there is also a test harness for a path system 33 | built on both the linear and rotational spline functionality 34 | [here](https://andrewwillmott.github.io/app/DemoApp.html?test=Spline%20Paths). 35 | This system is given a list of positions and times, and optionally speeds, 36 | directions, orientations and rotation speeds as constraints to hit. 37 | 38 | To build and run a test app, `make test`, or, add the Splines* files to your 39 | favourite IDE. 40 | 41 | In `extra` there is some code for drawing various spline features (as per the 42 | examples below and the demo). It's missing the draw and transform dependencies, 43 | but could be useful as reference. 44 | 45 | 46 | Examples 47 | -------- 48 | 49 | Splines from Points: 50 | 51 | ![points](images/points.gif "Splines from Points") 52 | 53 | Fast and Conservative Bounds: 54 | 55 | ![bounds](images/bounds.gif "Fast and Conservative Bounds") 56 | 57 | Closest Point on Spline: 58 | 59 | ![pick](images/pick.gif "Closest Point on Spline") 60 | 61 | Spline Intersections: 62 | 63 | ![pick](images/self-intersect.gif "Spline Intersections") 64 | 65 | Moving Points on Splines: 66 | 67 | ![agent](images/agent.gif "Moving Points on Splines") 68 | 69 | Subdivision and Joining: 70 | 71 | ![pick](images/subdivide.gif "Subdivision and Joining") 72 | -------------------------------------------------------------------------------- /extra/SplinesDraw.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // SplinesDraw.hpp 3 | // 4 | // Draw support for splines 5 | // 6 | // Andrew Willmott 7 | // 8 | 9 | #ifndef SL_SPLINES_DRAW_H 10 | #define SL_SPLINES_DRAW_H 11 | 12 | #include "Colour.hpp" 13 | #include "Splines.hpp" 14 | 15 | namespace SL 16 | { 17 | class Draw; 18 | 19 | enum SplineDrawMode 20 | { 21 | kSplineDrawModeNone, 22 | kSplineDrawModeT, // Show t value 23 | kSplineDrawModePosition, // Show position as rgb 24 | kSplineDrawModeVelocity, // Show ||velocity|| as green->red 25 | kSplineDrawModeAcceleration,// Show ||acceleration|| as green->red 26 | kSplineDrawModeCurvature, // Show curvature 27 | kSplineDrawModeLength, // Show length from green (0) to red (1) 28 | kSplineDrawModeLine, // Show lines that make up the rendered splines 29 | kNumSplineDrawModes 30 | }; 31 | 32 | // 2D 33 | void DrawSplines (Draw* dd, int numSplines, const Spline2 splines[], float tolerance = 0.05f); 34 | void DrawSplines (Draw* dd, int numSplines, const Spline2 splines[], SplineDrawMode drawMode, float drawScale = 1.0f, float tolerance = 0.05f); 35 | 36 | void DrawSplinesWithWidth (Draw* dd, int numSplines, const Spline2 splines[], float width, float tolerance = 0.05f); // draw two curves representing splines with given width 37 | void FillSplinesWithWidth (Draw* dd, int numSplines, const Spline2 splines[], float width, float tolerance = 0.05f); // fill area of splines with given width 38 | 39 | void DrawSplinePoints (Draw* dd, int numSplines, const Spline2 splines[], float r); 40 | void DrawSplineTangents (Draw* dd, int numSplines, const Spline2 splines[], const Colour& colour = kColourBlue); 41 | void DrawSplineFrames (Draw* dd, int numSplines, const Spline2 splines[], float size, float step = 0.2f, bool consistent = true); 42 | 43 | void DrawSplineCurvature (Draw* dd, int numSplines, const Spline2 splines[], float step = 0.2f, float maxR = 100); 44 | 45 | void DrawSplinesMesh (Draw* dd, int numSplines, const Spline2 splines[], float width, float tolerance = 0.05f); 46 | 47 | void DrawSplineExactBounds(Draw* dd, int numSplines, const Spline2 splines[], const Colour& colour = kColourOrange); 48 | void DrawSplineFastBounds (Draw* dd, int numSplines, const Spline2 splines[], const Colour& colour = kColourYellow); 49 | 50 | // 3D 51 | void DrawSplines (Draw* dd, int numSplines, const Spline3 splines[], float tolerance = 0.05f); 52 | void DrawSplines (Draw* dd, int numSplines, const Spline3 splines[], SplineDrawMode drawMode, float drawScale = 1.0f, float tolerance = 0.05f); 53 | 54 | void DrawSplinePoints (Draw* dd, int numSplines, const Spline3 splines[], float r, Vec3f normal = vl_z); 55 | void DrawSplineTangents (Draw* dd, int numSplines, const Spline3 splines[], const Colour& colour = kColourBlue); 56 | void DrawSplineFrames (Draw* dd, int numSplines, const Spline3 splines[], float size, float step = 0.2f, bool consistent = true); 57 | 58 | void DrawSplineCurvature (Draw* dd, int numSplines, const Spline3 splines[], float step = 0.2f, float maxR = 100); 59 | void DrawSplineTorsion (Draw* dd, int numSplines, const Spline3 splines[], float step = 0.2f, float scale = 100); 60 | 61 | void DrawSplinesMesh (Draw* dd, int numSplines, const Spline3 splines[], float width, float tolerance = 0.05f); 62 | 63 | void DrawSplinesWithWidth (Draw* dd, int numSplines, const Spline3 splines[], float width, float tolerance = 0.05f); // draw two curves representing splines with given width 64 | void FillSplinesWithWidth (Draw* dd, int numSplines, const Spline3 splines[], float width, float tolerance = 0.05f); // fill area of splines with given width 65 | 66 | void DrawSplineExactBounds(Draw* dd, int numSplines, const Spline3 splines[], const Colour& colour = kColourOrange); 67 | void DrawSplineFastBounds (Draw* dd, int numSplines, const Spline3 splines[], const Colour& colour = kColourYellow); 68 | 69 | // Helpers if you want to do your own spline->line conversions 70 | struct DLContext 71 | { 72 | Draw* dd; 73 | SplineDrawMode drawMode; 74 | float drawModeScale; 75 | }; 76 | 77 | // Adapters for SplinesToLines* 78 | void DrawSplineLines (int numLines, Vec2f lines[][2], const Spline2& spline, int i, int n, float ts[][2], void* dlContext); 79 | void DrawSplineLinesWithColours(int numLines, Vec2f lines[][2], const Spline2& spline, int i, int n, float ts[][2], void* dlContext); 80 | void DrawSplineLines (int numLines, Vec3f lines[][2], const Spline3& spline, int i, int n, float ts[][2], void* dlContext); 81 | void DrawSplineLinesWithColours(int numLines, Vec3f lines[][2], const Spline3& spline, int i, int n, float ts[][2], void* dlContext); 82 | 83 | // Draw 2D splines using 3D api 84 | void DrawSplineLines3D (int numLines, Vec2f lines[][2], const Spline2& spline, int i, int n, float ts[][2], void* dlContext); 85 | void DrawSplineLinesWithColours3D(int numLines, Vec2f lines[][2], const Spline2& spline, int i, int n, float ts[][2], void* dlContext); 86 | } 87 | 88 | #endif 89 | -------------------------------------------------------------------------------- /RotSplines.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // RotSplines.hpp 3 | // 4 | // Cubic spline utilities for rotations 5 | // 6 | // Andrew Willmott 7 | // 8 | 9 | #ifndef SL_ROT_SPLINES_H 10 | #define SL_ROT_SPLINES_H 11 | 12 | #include "Splines.hpp" 13 | 14 | namespace SL 15 | { 16 | // 2D 17 | typedef Spline1 RotSpline2; 18 | 19 | RotSpline2 BezierRotSpline (float r0, float r1, float r2, float r3); // Rotation angles in radians 20 | RotSpline2 HermiteRotSpline(float r0, float r1, float wv0, float wv1); // Start/end rotations + rot velocity in/out 21 | RotSpline2 LineRotSpline (float r0, float r1); // Simple linear interpolation 22 | 23 | float Rotation0(const RotSpline2& rs); // Start rotation angle 24 | float Rotation1(const RotSpline2& rs); // End rotation angle 25 | 26 | float RotVelocity0(const RotSpline2& rs); // Start rotational velocity (signed rad/s) 27 | float RotVelocity1(const RotSpline2& rs); // End rotational velocity (signed rad/s) 28 | 29 | float Rotation (const RotSpline2& rs, float t); // Interpolated rotation angle. Use with RRot2f() to get corresponding matrix. 30 | float RotVelocity (const RotSpline2& rs, float t); // Corresponding rotational velocity (rad/s). 31 | float RotAcceleration(const RotSpline2& rs, float t); // Corresponding rotational acceleration (rad/s/s). 32 | 33 | float AngSpeed (const RotSpline2& rs, float t); // Positive angular rotation (mostly for consistency with 3D api below) 34 | float AngAcceleration(const RotSpline2& rs, float t); // Positive angular acceleration 35 | 36 | int RotSplinesFromAngles(int numAngles, const float a[], RotSpline2 splines[], float tension = 0.0f, size_t stride = sizeof(float)); 37 | int RotSplinesFromDirs (int numDirs , const Vec2f d[], RotSpline2 splines[], float tension = 0.0f, size_t stride = sizeof(Vec2f)); 38 | // Creates a series of RotSplines that smoothly interpolate the given set of unit quaternions, parameterised by 'tension'. 39 | 40 | // 3D 41 | struct RotSpline3 42 | { 43 | Quatf q0; 44 | Vec3f w1; // Log(q0 -> q1) 45 | Vec3f w2; // Log(q1 -> q2) 46 | Vec3f w3; // Log(q2 -> q3) 47 | }; 48 | 49 | RotSpline3 BezierRotSpline (Quatf q0, Quatf q1, Quatf q2, Quatf q3); // q1/q2 are the usual control points 50 | RotSpline3 HermiteRotSpline(Quatf q0, Quatf q1, Vec3f w0, Vec3f w1); // w0/w1 here are rot velocity, e.g., from QuatDiff3(q-1, q+1) 51 | RotSpline3 LineRotSpline (Quatf q0, Quatf q1); // SLerp equivalent 52 | 53 | Quatf Rotation0(const RotSpline3& rs); // Start rotation 54 | Quatf Rotation1(const RotSpline3& rs); // End rotation 55 | 56 | Vec3f RotVelocity0(const RotSpline3& rs); // Start rotational velocity 57 | Vec3f RotVelocity1(const RotSpline3& rs); // End rotational velocity 58 | 59 | // Smooth rotation interpolation using 'cumulative' approach of KKS, with well-defined cheap(ish) derivatives. C2. 60 | Quatf Rotation (const RotSpline3& rs, float t); // Interpolated unit quaternion rotation. Use with RRot3f() to get corresponding matrix. 61 | Vec3f RotVelocity (const RotSpline3& rs, float t); // Derivative of Rotation() in tangent space (rotation axis x dtheta/dt). 62 | Quatf RotAcceleration(const RotSpline3& rs, float t); // Second derivative of Rotation() in tangent space (axis x d2theta/dt2 x scale). 63 | 64 | // These scalar angle variants match their 2D variants. Useful for comparison or you're interested in a measure of magnitude. 65 | float AngSpeed (const RotSpline3& rs, float t); // Angular speed: 2 x len(rv), with the 2 coming from the double cover. 66 | float AngAcceleration(const RotSpline3& rs, float t); // Angular acceleration: 2 x len(xyz(ra)). (Picking theta out of the log form of the derivative.) 67 | 68 | Quatf Rotation(Quatf q0, Quatf q1, Quatf q2, Quatf q3, float t); // Direct rotation interpolation using classic four Bezier-form quats 69 | Quatf Rotation(Quatf q0, Vec3f ld1, Vec3f ld2, Vec3f ld3, float t); // More efficient rotation interpolation using initial quat and three 1/3 log-form deltas 70 | 71 | // Alternate 'Shoemake' form of interpolation, using SLerp + De Casteljau algorithm. Not as amenable to derivatives/analysis. C1. 72 | Quatf RotationShoemake(const RotSpline3& rs, float t); 73 | Quatf RotationShoemake(Quatf q0, Quatf q1, Quatf q2, Quatf q3, float t); 74 | 75 | // Alternate 'Spherical Quadrangle' interpolation: simpler/faster version of Shoemake that replaces three linear slerps with one quadratic. C1 but notably jerkier at end points. 76 | Quatf RotationSQuad(const RotSpline3& rs, float t); 77 | Quatf RotationSQuad(Quatf q0, Quatf q1, Quatf q2, Quatf q3, float t); // Note: technically need to set q1/q2 with 1/2 factor rather than 1/3, but not found much difference. 78 | 79 | void Split(const RotSpline3& rs, float t, RotSpline3* rs0, RotSpline3* rs1); // Splits 'rs' into two halves (at t = 0.5) and stores the results in rs0/1 80 | bool Join (const RotSpline3& rs0, const RotSpline3& rs1, RotSpline3* rs); // Joins two splines that were formerly Split(). Assumes t=0.5, returns false if the source splines don't match up. 81 | 82 | int RotSplinesFromQuats(int numQuats, const Quatf qi[], RotSpline3 splines[], float tension = 0.0f, size_t stride = sizeof(Quatf)); 83 | // Creates a series of RotSplines that smoothly interpolate the given set of unit quaternions, parameterised by 'tension'. 84 | } 85 | 86 | 87 | // Inlines 88 | 89 | // would be nice if they ever bothered to add a robust function alias 90 | inline SL::RotSpline2 SL::BezierRotSpline (float w0, float w1, float w2, float w3) 91 | { 92 | return BezierSpline(w0, w1, w2, w3); 93 | } 94 | 95 | inline SL::RotSpline2 SL::HermiteRotSpline(float w0, float w1, float wv0, float wv1) 96 | { 97 | return HermiteSpline(w0, w1, wv0, wv1); 98 | } 99 | 100 | inline SL::RotSpline2 SL::LineRotSpline(float w0, float w1) 101 | { 102 | return LineSpline(w0, w1); 103 | } 104 | 105 | inline float SL::Rotation0(const RotSpline2& rs) 106 | { 107 | return Position0(rs); 108 | } 109 | 110 | inline float SL::Rotation1(const RotSpline2& rs) 111 | { 112 | return Position1(rs); 113 | } 114 | 115 | inline float SL::RotVelocity0(const RotSpline2& rs) 116 | { 117 | return Velocity0(rs); 118 | } 119 | 120 | inline float SL::RotVelocity1(const RotSpline2& rs) 121 | { 122 | return Velocity1(rs); 123 | } 124 | 125 | inline float SL::Rotation(const RotSpline2& rs, float t) 126 | { 127 | return Position(rs, t); 128 | } 129 | 130 | inline float SL::RotVelocity(const RotSpline2& rs, float t) 131 | { 132 | return Velocity(rs, t); 133 | } 134 | 135 | inline float SL::RotAcceleration(const RotSpline2& rs, float t) 136 | { 137 | return Acceleration(rs, t); 138 | } 139 | 140 | inline float SL::AngSpeed(const RotSpline2& rs, float t) 141 | { 142 | return fabsf(Velocity(rs, t)); 143 | } 144 | 145 | inline float SL::AngAcceleration(const RotSpline2& rs, float t) 146 | { 147 | return fabsf(Acceleration(rs, t)); 148 | } 149 | 150 | inline float SL::AngSpeed(const RotSpline3& rs, float t) 151 | { 152 | return 2 * len(RotVelocity(rs, t)); 153 | } 154 | 155 | inline float SL::AngAcceleration(const RotSpline3& rs, float t) 156 | { 157 | return 2 * len(xyz(RotAcceleration(rs, t))); 158 | } 159 | 160 | #endif 161 | -------------------------------------------------------------------------------- /SplinesTest.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // SplinesTest.cpp 3 | // 4 | // Cubic spline utilities test 5 | // 6 | // Andrew Willmott 7 | // 8 | 9 | // Simple example that exercises the 2D/3D apis by creating a run of splines from 10 | // a set of points, and then querying them in different ways. 11 | 12 | #include "Splines.hpp" 13 | #include "RotSplines.hpp" 14 | 15 | #include 16 | 17 | using namespace SL; 18 | 19 | void TestSplines2() 20 | { 21 | printf("+ Spline2\n"); 22 | 23 | const Vec2f points[] = 24 | { 25 | { 1.14978f, -137.651f }, 26 | { -174.87f, -78.9264f }, 27 | { 51.2561f, 90.6417f }, 28 | { -183.824f, -39.0125f }, 29 | { 22.3669f, 21.0507f }, 30 | { -63.4809f, 145.797f }, 31 | { 16.1387f, -137.128f }, 32 | { -49.4056f, 159.712f }, 33 | { 7.183f, -90.0083f }, 34 | { -164.355f, -189.983f }, 35 | { -180.224f, 42.4843f }, 36 | { 102.902f, 118.445f }, 37 | { 8.07961f, 188.87f }, 38 | { 160.243f, -126.232f }, 39 | { -125.71f, -123.763f }, 40 | { -102.967f, 18.3226f }, 41 | }; 42 | const int numPoints = sizeof(points) / sizeof(points[0]); 43 | 44 | Spline2 splines[numPoints + 1]; 45 | 46 | int numSplines = SplinesFromPoints(numPoints, points, splines); 47 | float sumLen = 0.0f; 48 | 49 | printf("\ninfo:\n"); 50 | for (int i = 0; i < numSplines; i++) 51 | { 52 | float len = Length(splines[i], 0.01f); 53 | Bounds2 bb = ExactBounds(splines[i]); 54 | 55 | printf(" %2d: length: %5.1f, bounds [%6.1f, %6.1f], [%6.1f, %6.1f]\n", i, len, bb.mMin.x, bb.mMin.y, bb.mMax.x, bb.mMax.y); 56 | 57 | sumLen += len; 58 | } 59 | printf("\ntotal length: %6.1f\n", sumLen); 60 | 61 | printf("\ninterpolation:\n"); 62 | for (int is = 4; is < 6; is++) 63 | for (float ts = 0.1f; ts <= 1.0f; ts += 0.2f) 64 | { 65 | Vec2f ps = Position (splines[is], ts); 66 | Vec2f vs = Velocity (splines[is], ts); 67 | float cs = Curvature(splines[is], ts); 68 | 69 | printf(" i %d, t %.1f: position [%6.1f, %6.1f], velocity [%6.1f, %6.1f], curvature %6.3f\n", is, ts, ps.x, ps.y, vs.x, vs.y, cs); 70 | } 71 | 72 | const Vec2f queryPoints[] = 73 | { 74 | { 83.7624f, -122.161f }, 75 | { 126.166f, -40.4692f }, 76 | { -36.1414f, -9.16203f }, 77 | { -61.7727f, -131.043f }, 78 | }; 79 | 80 | printf("\nclosest points:\n"); 81 | for (Vec2f qp : queryPoints) 82 | { 83 | int index; 84 | float t = FindClosestPoint(qp, numSplines, splines, &index); 85 | 86 | Vec2f cp = Position(splines[index], t); 87 | 88 | printf(" to [%6.1f, %6.1f]: i %2d, t %4.2f, point [%6.1f, %6.1f]\n", qp.x, qp.y, index, t, cp.x, cp.y); 89 | } 90 | 91 | float intT[32][2]; 92 | int intI[32][2]; 93 | 94 | int numIntersections = FindSplineIntersections(numSplines / 2, splines, sizeof(intT) / sizeof(intT[0]), intI, intT, 1.0f); 95 | 96 | printf("\nself-intersections:\n"); 97 | for (int i = 0; i < numIntersections; i++) 98 | printf(" i0 %d, t0 %4.2f, i1 %d, t1 %4.2f\n", intI[i][0], intT[i][0], intI[i][1], intT[i][1]); 99 | 100 | printf("\n"); 101 | } 102 | 103 | void TestSplines3() 104 | { 105 | printf("+ Spline3\n"); 106 | 107 | Vec3f points[] = 108 | { 109 | { 1.14978f , -137.651f, 11.1f }, 110 | { -174.87f , -78.9264f, 22.2f }, 111 | { 51.2561f , 90.6417f , 66.6f }, 112 | { -183.824f, -39.0125f, 88.8f }, 113 | { 22.3669f , 21.0507f , 66.6f }, 114 | { -63.4809f, 145.797f , 44.4f }, 115 | { 16.1387f , -137.128f, -22.2f }, 116 | { -49.4056f, 159.712f , 11.1f }, 117 | { 7.183f , -90.0083f, 11.1f }, 118 | { -164.355f, -189.983f, 33.3f }, 119 | { -180.224f, 42.4843f , 11.1f }, 120 | { 102.902f , 118.445f , 44.4f }, 121 | { 8.07961f , 188.87f , 66.6f }, 122 | { 160.243f , -126.232f, 88.8f }, 123 | { -125.71f , -123.763f, 55.5f }, 124 | { -102.967f, 18.3226f , 11.1f }, 125 | }; 126 | const int numPoints = sizeof(points) / sizeof(points[0]); 127 | 128 | Spline3 splines[numPoints + 1]; 129 | 130 | int numSplines = SplinesFromPoints(numPoints, points, splines); 131 | float sumLen = 0.0f; 132 | 133 | printf("\ninfo:\n"); 134 | for (int i = 0; i < numSplines; i++) 135 | { 136 | float len = Length(splines[i], 0.01f); 137 | Bounds3 bb = ExactBounds(splines[i]); 138 | 139 | printf(" %2d: length %5.1f, bounds [%6.1f, %6.1f, %6.1f], [%6.1f, %6.1f, %6.1f]\n", i, len, bb.mMin.x, bb.mMin.y, bb.mMin.z, bb.mMax.x, bb.mMax.y, bb.mMax.z); 140 | 141 | sumLen += len; 142 | } 143 | printf("\ntotal length: %6.1f\n", sumLen); 144 | 145 | printf("\ninterpolation:\n"); 146 | for (int is = 4; is < 6; is++) 147 | for (float ts = 0.1f; ts <= 1.0f; ts += 0.2f) 148 | { 149 | Vec3f ps = Position (splines[is], ts); 150 | Vec3f vs = Velocity (splines[is], ts); 151 | float cs = Curvature(splines[is], ts); 152 | float rs = Torsion (splines[is], ts); 153 | printf(" i %d, t %.1f: position [%6.1f, %6.1f, %6.1f], velocity [%6.1f, %6.1f, %6.1f], curvature %6.3f, torsion %6.3f\n", is, ts, ps.x, ps.y, ps.z, vs.x, vs.y, vs.z, cs, rs); 154 | } 155 | 156 | const Vec3f queryPoints[] = 157 | { 158 | { 83.7624f , -122.161f, 22.2f }, 159 | { 126.166f , -40.4692f, 11.1f }, 160 | { -36.1414f, -9.16203f, 88.8f }, 161 | { -61.7727f, -131.043f, 66.6f }, 162 | }; 163 | 164 | printf("\nclosest points:\n"); 165 | for (Vec3f qp : queryPoints) 166 | { 167 | int index; 168 | float t = FindClosestPoint(qp, numSplines, splines, &index); 169 | 170 | Vec3f cp = Position(splines[index], t); 171 | 172 | printf(" to [%6.1f, %6.1f, %6.1f]: i %2d, t %4.2f, point [%6.1f, %6.1f, %6.1f]\n", qp.x, qp.y, qp.z, index, t, cp.x, cp.y, cp.z); 173 | } 174 | 175 | // Flatten sections so we get some self-intersections. 176 | points[1].z = 11.1f; 177 | points[2].z = 11.1f; 178 | points[5].z = 11.1f; 179 | points[6].z = 11.1f; 180 | 181 | SplinesFromPoints(numPoints, points, splines, 1.0f); // tension=1 forces flat sections for intersections 182 | 183 | float intT[32][2]; 184 | int intI[32][2]; 185 | 186 | int numIntersections = FindSplineIntersections(numSplines / 2, splines, sizeof(intT) / sizeof(intT[0]), intI, intT, 1.0f); 187 | 188 | printf("\nself-intersections:\n"); 189 | for (int i = 0; i < numIntersections; i++) 190 | printf(" i0 %d, t0 %4.2f, i1 %d, t1 %4.2f\n", intI[i][0], intT[i][0], intI[i][1], intT[i][1]); 191 | 192 | printf("\n"); 193 | } 194 | 195 | void TestRotSplines2() 196 | { 197 | printf("+ RotSpline2\n"); 198 | 199 | #if 1 200 | const Vec2f dirs[] = 201 | { 202 | { 1, 0 }, 203 | { 0, 1 }, 204 | { -1, 0 }, 205 | { 0, -1 }, 206 | { 1, 0 }, 207 | { 0, -1 }, 208 | }; 209 | const int numDirs = sizeof(dirs) / sizeof(dirs[0]); 210 | 211 | RotSpline2 splines[numDirs + 1]; 212 | 213 | int numSplines = RotSplinesFromDirs(numDirs, dirs, splines); 214 | #else 215 | const float angles[] = 216 | { 217 | 0, 218 | vlf_pi * 4 + vlf_halfPi, 219 | vlf_pi, 220 | -vlf_halfPi, 221 | vlf_twoPi, 222 | vlf_twoPi - vlf_halfPi, 223 | }; 224 | const int numAngles = sizeof(angles) / sizeof(angles[0]); 225 | 226 | RotSpline2 splines[numAngles + 1]; 227 | 228 | int numSplines = RotSplinesFromAngles(numAngles, angles, splines); 229 | #endif 230 | 231 | printf("\ninterpolation:\n"); 232 | for (int is = 0; is < numSplines; is++) 233 | for (float ts = 0.1f; ts <= 1.0f; ts += 0.2f) 234 | { 235 | float w = Rotation (splines[is], ts); 236 | float wv = RotVelocity(splines[is], ts); 237 | 238 | Vec2f v(sinf(w), cosf(w)); 239 | 240 | printf(" i %d, t %0.2f: direction [%6.3f, %6.3f], rot speed %.2f rad/s\n", is, ts, v.x, v.y, 2.0f * wv); 241 | } 242 | 243 | printf("\n"); 244 | } 245 | 246 | void TestRotSplines3() 247 | { 248 | printf("+ RotSpline3\n"); 249 | 250 | const Quatf quats[] = 251 | { 252 | { 1, 0, 0, 0 }, 253 | { 0, 1, 0, 0 }, 254 | { 0, 0, 1, 0 }, 255 | { 0, 0, 0, 1 }, 256 | { 0.7071, 0.7071, 0, 0 }, 257 | { 0, 0.7071, 0.7071, 0 }, 258 | { 0, 0, 0.7071, 0.7071 }, 259 | { 0.7071, 0, 0, 0.7071 }, 260 | }; 261 | const int numQuats = sizeof(quats) / sizeof(quats[0]); 262 | 263 | RotSpline3 splines[numQuats + 1]; 264 | 265 | int numSplines = RotSplinesFromQuats(numQuats, quats, splines); 266 | 267 | printf("\ninterpolation:\n"); 268 | for (int is = 0; is < numSplines; is++) 269 | for (float ts = 0.1f; ts <= 1.0f; ts += 0.2f) 270 | { 271 | Quatf q = Rotation (splines[is], ts); 272 | Vec3f w = RotVelocity(splines[is], ts); 273 | 274 | printf(" i %d, t %0.2f: quat [%6.3f, %6.3f, %6.3f, %6.3f], rot speed %.2f rad/s\n", is, ts, q.x, q.y, q.z, q.w, 2.0f * len(w)); 275 | } 276 | 277 | printf("\n"); 278 | } 279 | 280 | int main(int argc, const char* argv[]) 281 | { 282 | (void) argc; (void) argv; 283 | 284 | TestSplines2(); 285 | TestSplines3(); 286 | 287 | TestRotSplines2(); 288 | TestRotSplines3(); 289 | 290 | return 0; 291 | } 292 | -------------------------------------------------------------------------------- /test-ref.txt: -------------------------------------------------------------------------------- 1 | + Spline2 2 | 3 | info: 4 | 0: length: 193.9, bounds [-175.2, -137.7], [ 1.1, -78.9] 5 | 1: length: 284.7, bounds [-174.9, -78.9], [ 51.3, 90.6] 6 | 2: length: 269.4, bounds [-183.8, -39.0], [ 51.3, 90.9] 7 | 3: length: 219.2, bounds [-183.9, -41.0], [ 22.4, 21.1] 8 | 4: length: 166.1, bounds [ -63.5, 21.1], [ 24.9, 149.5] 9 | 5: length: 294.3, bounds [ -63.5, -137.1], [ 16.1, 145.8] 10 | 6: length: 304.1, bounds [ -49.4, -137.1], [ 16.2, 159.7] 11 | 7: length: 259.2, bounds [ -49.4, -90.0], [ 10.2, 159.9] 12 | 8: length: 212.1, bounds [-164.4, -194.5], [ 7.2, -90.0] 13 | 9: length: 245.3, bounds [-203.0, -190.0], [-164.4, 42.5] 14 | 10: length: 296.6, bounds [-180.2, 42.5], [ 102.9, 118.4] 15 | 11: length: 148.6, bounds [ 7.6, 118.4], [ 107.6, 199.6] 16 | 12: length: 354.3, bounds [ 8.1, -126.2], [ 162.3, 188.9] 17 | 13: length: 298.0, bounds [-125.7, -154.8], [ 160.2, -123.8] 18 | 14: length: 154.8, bounds [-141.2, -123.8], [-103.0, 18.3] 19 | 20 | total length: 3700.5 21 | 22 | interpolation: 23 | i 4, t 0.1: position [ 24.9, 32.7], velocity [ -7.9, 139.0], curvature 0.030 24 | i 4, t 0.3: position [ 12.9, 66.6], velocity [-102.9, 189.7], curvature 0.005 25 | i 4, t 0.5: position [ -12.6, 104.9], velocity [-143.0, 183.8], curvature 0.003 26 | i 4, t 0.7: position [ -40.7, 136.3], velocity [-128.2, 121.2], curvature 0.006 27 | i 4, t 0.9: position [ -60.3, 149.5], velocity [ -58.5, 1.8], curvature 0.211 28 | i 5, t 0.1: position [ -61.6, 131.4], velocity [ 39.8, -203.8], curvature 0.004 29 | i 5, t 0.3: position [ -47.2, 72.6], velocity [ 97.8, -364.3], curvature 0.000 30 | i 5, t 0.5: position [ -24.9, -6.4], velocity [ 118.4, -406.4], curvature 0.000 31 | i 5, t 0.7: position [ -2.3, -82.0], velocity [ 101.8, -329.9], curvature 0.000 32 | i 5, t 0.9: position [ 13.3, -130.5], velocity [ 48.0, -135.0], curvature 0.004 33 | 34 | closest points: 35 | to [ 83.8, -122.2]: i 13, t 0.33, point [ 85.7, -154.1] 36 | to [ 126.2, -40.5]: i 12, t 0.68, point [ 135.5, -35.8] 37 | to [ -36.1, -9.2]: i 3, t 0.68, point [ -36.1, -9.1] 38 | to [ -61.8, -131.0]: i 0, t 0.34, point [ -61.0, -126.3] 39 | 40 | self-intersections: 41 | i0 1, t0 0.26, i1 3, t1 0.35 42 | i0 1, t0 0.74, i1 4, t1 0.29 43 | i0 1, t0 0.57, i1 5, t1 0.40 44 | i0 1, t0 0.61, i1 6, t1 0.57 45 | i0 2, t0 0.27, i1 4, t1 0.33 46 | i0 2, t0 0.43, i1 5, t1 0.37 47 | i0 2, t0 0.38, i1 6, t1 0.60 48 | i0 3, t0 0.73, i1 5, t1 0.50 49 | i0 3, t0 0.78, i1 6, t1 0.48 50 | i0 4, t0 0.73, i1 6, t1 0.85 51 | 52 | + Spline3 53 | 54 | info: 55 | 0: length 194.5, bounds [-175.2, -137.7, 11.1], [ 1.1, -78.9, 22.2] 56 | 1: length 288.7, bounds [-174.9, -78.9, 22.2], [ 51.3, 90.6, 66.6] 57 | 2: length 271.1, bounds [-183.8, -39.0, 66.6], [ 51.3, 90.9, 88.8] 58 | 3: length 220.4, bounds [-183.9, -41.0, 66.6], [ 22.4, 21.1, 88.8] 59 | 4: length 168.9, bounds [ -63.5, 21.1, 44.4], [ 24.9, 149.5, 66.6] 60 | 5: length 302.2, bounds [ -63.5, -137.1, -22.2], [ 16.1, 145.8, 44.4] 61 | 6: length 306.3, bounds [ -49.4, -137.1, -22.8], [ 16.2, 159.7, 11.1] 62 | 7: length 259.6, bounds [ -49.4, -90.0, 10.2], [ 10.2, 159.9, 12.9] 63 | 8: length 213.4, bounds [-164.4, -194.5, 11.1], [ 7.2, -90.0, 33.3] 64 | 9: length 246.4, bounds [-203.0, -190.0, 11.0], [-164.4, 42.5, 33.3] 65 | 10: length 298.6, bounds [-180.2, 42.5, 11.1], [ 102.9, 118.4, 44.4] 66 | 11: length 150.5, bounds [ 7.6, 118.4, 44.4], [ 107.6, 199.6, 66.6] 67 | 12: length 355.1, bounds [ 8.1, -126.2, 66.6], [ 162.3, 188.9, 88.9] 68 | 13: length 300.0, bounds [-125.7, -154.8, 55.5], [ 160.2, -123.8, 88.8] 69 | 14: length 161.1, bounds [-141.2, -123.8, 11.1], [-103.0, 18.3, 55.5] 70 | 71 | total length: 3736.7 72 | 73 | interpolation: 74 | i 4, t 0.1: position [ 24.9, 32.7, 64.6], velocity [ -7.9, 139.0, -18.4], curvature 0.030, torsion -0.002 75 | i 4, t 0.3: position [ 12.9, 66.6, 61.3], velocity [-102.9, 189.7, -14.9], curvature 0.005, torsion -0.004 76 | i 4, t 0.5: position [ -12.6, 104.9, 58.3], velocity [-143.0, 183.8, -16.6], curvature 0.003, torsion -0.008 77 | i 4, t 0.7: position [ -40.7, 136.3, 54.3], velocity [-128.2, 121.2, -23.8], curvature 0.007, torsion -0.008 78 | i 4, t 0.9: position [ -60.3, 149.5, 48.4], velocity [ -58.5, 1.8, -36.2], curvature 0.168, torsion -0.004 79 | i 5, t 0.1: position [ -61.6, 131.4, 39.1], velocity [ 39.8, -203.8, -61.1], curvature 0.005, torsion -0.001 80 | i 5, t 0.3: position [ -47.2, 72.6, 24.5], velocity [ 97.8, -364.3, -81.5], curvature 0.001, torsion -0.002 81 | i 5, t 0.5: position [ -24.9, -6.4, 7.6], velocity [ 118.4, -406.4, -84.6], curvature 0.000, torsion -0.010 82 | i 5, t 0.7: position [ -2.3, -82.0, -8.2], velocity [ 101.8, -329.9, -70.4], curvature 0.000, torsion -0.011 83 | i 5, t 0.9: position [ 13.3, -130.5, -19.4], velocity [ 48.0, -135.0, -38.9], curvature 0.008, torsion -0.004 84 | 85 | closest points: 86 | to [ 83.8, -122.2, 22.2]: i 13, t 0.35, point [ 79.6, -154.4, 81.6] 87 | to [ 126.2, -40.5, 11.1]: i 12, t 0.67, point [ 133.8, -32.3, 85.6] 88 | to [ -36.1, -9.2, 88.8]: i 3, t 0.68, point [ -37.5, -9.6, 75.3] 89 | to [ -61.8, -131.0, 66.6]: i 13, t 0.74, point [ -63.9, -142.0, 66.1] 90 | 91 | self-intersections: 92 | i0 1, t0 0.59, i1 5, t1 0.44 93 | i0 1, t0 0.62, i1 6, t1 0.56 94 | i0 2, t0 0.96, i1 3, t1 0.05 95 | i0 5, t0 0.93, i1 6, t1 0.07 96 | i0 5, t0 0.95, i1 6, t1 0.05 97 | 98 | + RotSpline2 99 | 100 | interpolation: 101 | i 0, t 0.10: direction [ 0.093, 0.996], rot speed 2.15 rad/s 102 | i 0, t 0.30: direction [ 0.348, 0.937], rot speed 3.03 rad/s 103 | i 0, t 0.50: direction [ 0.634, 0.773], rot speed 3.53 rad/s 104 | i 0, t 0.70: direction [ 0.867, 0.498], rot speed 3.66 rad/s 105 | i 0, t 0.90: direction [ 0.987, 0.163], rot speed 3.41 rad/s 106 | i 1, t 0.10: direction [ 0.988, -0.156], rot speed 3.14 rad/s 107 | i 1, t 0.30: direction [ 0.891, -0.454], rot speed 3.14 rad/s 108 | i 1, t 0.50: direction [ 0.707, -0.707], rot speed 3.14 rad/s 109 | i 1, t 0.70: direction [ 0.454, -0.891], rot speed 3.14 rad/s 110 | i 1, t 0.90: direction [ 0.156, -0.988], rot speed 3.14 rad/s 111 | i 2, t 0.10: direction [-0.156, -0.988], rot speed 3.14 rad/s 112 | i 2, t 0.30: direction [-0.454, -0.891], rot speed 3.14 rad/s 113 | i 2, t 0.50: direction [-0.707, -0.707], rot speed 3.14 rad/s 114 | i 2, t 0.70: direction [-0.891, -0.454], rot speed 3.14 rad/s 115 | i 2, t 0.90: direction [-0.988, -0.156], rot speed 3.14 rad/s 116 | i 3, t 0.10: direction [-0.985, 0.170], rot speed 3.68 rad/s 117 | i 3, t 0.30: direction [-0.842, 0.540], rot speed 4.18 rad/s 118 | i 3, t 0.50: direction [-0.556, 0.831], rot speed 3.93 rad/s 119 | i 3, t 0.70: direction [-0.238, 0.971], rot speed 2.92 rad/s 120 | i 3, t 0.90: direction [-0.030, 1.000], rot speed 1.16 rad/s 121 | i 4, t 0.10: direction [-0.037, 0.999], rot speed -1.43 rad/s 122 | i 4, t 0.30: direction [-0.286, 0.958], rot speed -3.44 rad/s 123 | i 4, t 0.50: direction [-0.634, 0.773], rot speed -4.32 rad/s 124 | i 4, t 0.70: direction [-0.898, 0.439], rot speed -4.07 rad/s 125 | i 4, t 0.90: direction [-0.994, 0.107], rot speed -2.69 rad/s 126 | 127 | + RotSpline3 128 | 129 | interpolation: 130 | i 0, t 0.10: quat [ 0.998, 0.065, -0.000, -0.004], rot speed 2.03 rad/s 131 | i 0, t 0.30: quat [ 0.930, 0.368, 0.001, -0.027], rot speed 4.00 rad/s 132 | i 0, t 0.50: quat [ 0.684, 0.728, 0.007, -0.051], rot speed 4.58 rad/s 133 | i 0, t 0.70: quat [ 0.317, 0.947, 0.009, -0.052], rot speed 3.79 rad/s 134 | i 0, t 0.90: quat [ 0.042, 0.999, 0.003, -0.023], rot speed 1.66 rad/s 135 | i 1, t 0.10: quat [ 0.001, 0.999, 0.044, 0.024], rot speed 1.75 rad/s 136 | i 1, t 0.30: quat [ 0.018, 0.941, 0.332, 0.056], rot speed 3.97 rad/s 137 | i 1, t 0.50: quat [ 0.046, 0.704, 0.706, 0.062], rot speed 4.72 rad/s 138 | i 1, t 0.70: quat [ 0.052, 0.331, 0.942, 0.038], rot speed 3.96 rad/s 139 | i 1, t 0.90: quat [ 0.024, 0.043, 0.999, 0.007], rot speed 1.75 rad/s 140 | i 2, t 0.10: quat [-0.021, -0.005, 0.999, 0.044], rot speed 1.73 rad/s 141 | i 2, t 0.30: quat [-0.033, -0.028, 0.942, 0.332], rot speed 3.96 rad/s 142 | i 2, t 0.50: quat [-0.010, -0.041, 0.707, 0.706], rot speed 4.72 rad/s 143 | i 2, t 0.70: quat [ 0.017, -0.033, 0.333, 0.942], rot speed 3.96 rad/s 144 | i 2, t 0.90: quat [ 0.015, -0.015, 0.044, 0.999], rot speed 1.73 rad/s 145 | i 3, t 0.10: quat [ 0.016, 0.045, -0.001, 0.999], rot speed 1.68 rad/s 146 | i 3, t 0.30: quat [ 0.206, 0.255, 0.006, 0.945], rot speed 3.88 rad/s 147 | i 3, t 0.50: quat [ 0.478, 0.505, 0.025, 0.718], rot speed 4.65 rad/s 148 | i 3, t 0.70: quat [ 0.666, 0.658, 0.032, 0.350], rot speed 3.98 rad/s 149 | i 3, t 0.90: quat [ 0.714, 0.698, 0.014, 0.054], rot speed 1.88 rad/s 150 | i 4, t 0.10: quat [ 0.686, 0.728, 0.013, -0.015], rot speed 1.14 rad/s 151 | i 4, t 0.30: quat [ 0.588, 0.792, 0.160, -0.041], rot speed 2.54 rad/s 152 | i 4, t 0.50: quat [ 0.414, 0.822, 0.387, -0.051], rot speed 3.07 rad/s 153 | i 4, t 0.70: quat [ 0.204, 0.782, 0.588, -0.040], rot speed 2.67 rad/s 154 | i 4, t 0.90: quat [ 0.038, 0.719, 0.694, -0.016], rot speed 1.36 rad/s 155 | i 5, t 0.10: quat [-0.016, 0.693, 0.720, 0.036], rot speed 1.30 rad/s 156 | i 5, t 0.30: quat [-0.032, 0.589, 0.785, 0.191], rot speed 2.56 rad/s 157 | i 5, t 0.50: quat [-0.026, 0.397, 0.827, 0.396], rot speed 3.00 rad/s 158 | i 5, t 0.70: quat [-0.010, 0.177, 0.800, 0.574], rot speed 2.57 rad/s 159 | i 5, t 0.90: quat [-0.001, 0.023, 0.733, 0.680], rot speed 1.30 rad/s 160 | i 6, t 0.10: quat [ 0.022, -0.001, 0.680, 0.733], rot speed 1.30 rad/s 161 | i 6, t 0.30: quat [ 0.172, -0.005, 0.570, 0.803], rot speed 2.55 rad/s 162 | i 6, t 0.50: quat [ 0.388, -0.004, 0.391, 0.835], rot speed 2.98 rad/s 163 | i 6, t 0.70: quat [ 0.579, -0.001, 0.186, 0.794], rot speed 2.57 rad/s 164 | i 6, t 0.90: quat [ 0.688, 0.000, 0.033, 0.725], rot speed 1.30 rad/s 165 | 166 | -------------------------------------------------------------------------------- /VLMini.hpp: -------------------------------------------------------------------------------- 1 | #ifndef VL_MINI_H 2 | #define VL_MINI_H 3 | 4 | #include 5 | 6 | #define VL_ASSERT(X) 7 | 8 | template inline T sqr(T x) { return x * x; } 9 | 10 | template inline T vl_min(T a, T b) { return a < b ? a : b; } 11 | template inline T vl_max(T a, T b) { return a > b ? a : b; } 12 | 13 | constexpr float vlf_pi = 3.14159265358979323846f; 14 | constexpr float vlf_halfPi = 0.5 * vlf_pi; 15 | constexpr float vlf_twoPi = 2 * vlf_pi; 16 | 17 | #define VI(T) T operator[](int i) const { return (&x)[i]; } T& operator[](int i){ return (&x)[i]; } 18 | 19 | inline float len (float x) { return fabsf(x); } 20 | inline float sqrlen(float x) { return x * x; } 21 | 22 | struct Vec2f { float x; float y; Vec2f() {}; Vec2f(float xi, float yi) : x(xi), y(yi) {}; VI(float) }; 23 | struct Vec3f { float x; float y; float z; Vec3f() {}; Vec3f(float xi, float yi, float zi) : x(xi), y(yi), z(zi) {}; VI(float) }; 24 | struct Vec4f { float x; float y; float z; float w; Vec4f() {}; Vec4f(float xi, float yi, float zi, float wi) : x(xi), y(yi), z(zi), w(wi) {}; VI(float) }; 25 | 26 | struct Vec2d { double x; double y; Vec2d() {}; Vec2d(double xi, double yi) : x(xi), y(yi) {}; VI(double) }; 27 | struct Vec3d { double x; double y; double z; Vec3d() {}; Vec3d(double xi, double yi, double zi) : x(xi), y(yi), z(zi) {}; VI(double) }; 28 | struct Vec4d { double x; double y; double z; double w; Vec4d() {}; Vec4d(double xi, double yi, double zi, double wi) : x(xi), y(yi), z(zi), w(wi) {}; VI(double) }; 29 | 30 | struct Mat2f { Vec2f x; Vec2f y; Mat2f() {}; Mat2f(Vec2f xi, Vec2f yi) : x(xi), y(yi) {}; VI(Vec2f) }; 31 | struct Mat3f { Vec3f x; Vec3f y; Vec3f z; Mat3f() {}; Mat3f(Vec3f xi, Vec3f yi, Vec3f zi) : x(xi), y(yi), z(zi) {}; VI(Vec3f) }; 32 | struct Mat4f { Vec4f x; Vec4f y; Vec4f z; Vec4f w; Mat4f() {}; Mat4f(Vec4f xi, Vec4f yi, Vec4f zi, Vec4f wi) : x(xi), y(yi), z(zi), w(wi) {}; VI(Vec4f) }; 33 | 34 | inline Vec2f operator+ (Vec2f v) { return { +v.x, +v.y }; } 35 | inline Vec2f operator- (Vec2f v) { return { -v.x, -v.y }; } 36 | inline Vec2f operator+ (Vec2f a, Vec2f b) { return { a.x + b.x, a.y + b.y }; } 37 | inline Vec2f operator- (Vec2f a, Vec2f b) { return { a.x - b.x, a.y - b.y }; } 38 | inline Vec2f operator* (Vec2f a, Vec2f b) { return { a.x * b.x, a.y * b.y }; } 39 | inline Vec2f operator/ (Vec2f a, Vec2f b) { return { a.x / b.x, a.y / b.y }; } 40 | inline Vec2f operator* (float s, Vec2f a) { return { s * a.x, s * a.y }; } 41 | inline Vec2f operator* (Vec2f a, float s) { return { s * a.x, s * a.y }; } 42 | inline Vec2f operator/ (float s, Vec2f a) { return { s / a.x, s / a.y }; } 43 | inline Vec2f operator/ (Vec2f a, float s) { return { s / a.x, s / a.y }; } 44 | inline Vec2f& operator+=(Vec2f& a, Vec2f b) { a.x += b.x; a.y += b.y; return a; } 45 | inline Vec2f& operator-=(Vec2f& a, Vec2f b) { a.x -= b.x; a.y -= b.y; return a; } 46 | inline Vec2f& operator*=(Vec2f& a, Vec2f b) { a.x *= b.x; a.y *= b.y; return a; } 47 | inline Vec2f& operator/=(Vec2f& a, Vec2f b) { a.x /= b.x; a.y /= b.y; return a; } 48 | inline Vec2f& operator*=(Vec2f& a, float s) { a.x *= s ; a.y *= s ; return a; } 49 | inline Vec2f& operator/=(Vec2f& a, float s) { a.x /= s ; a.y /= s ; return a; } 50 | 51 | inline bool operator==(Vec3f a, Vec3f b) { return a.x == b.x && a.y == b.y && a.z == b.z; } 52 | inline bool operator!=(Vec3f a, Vec3f b) { return a.x != b.x || a.y != b.y || a.z != b.z; } 53 | inline Vec3f operator+ (Vec3f v) { return { +v.x, +v.y, +v.z }; } 54 | inline Vec3f operator- (Vec3f v) { return { -v.x, -v.y, -v.z }; } 55 | inline Vec3f operator+ (Vec3f a, Vec3f b) { return { a.x + b.x, a.y + b.y, a.z + b.z }; } 56 | inline Vec3f operator- (Vec3f a, Vec3f b) { return { a.x - b.x, a.y - b.y, a.z - b.z }; } 57 | inline Vec3f operator* (Vec3f a, Vec3f b) { return { a.x * b.x, a.y * b.y, a.z * b.z }; } 58 | inline Vec3f operator/ (Vec3f a, Vec3f b) { return { a.x / b.x, a.y / b.y, a.z / b.z }; } 59 | inline Vec3f operator* (float s, Vec3f a) { return { s * a.x, s * a.y, s * a.z }; } 60 | inline Vec3f operator* (Vec3f a, float s) { return { s * a.x, s * a.y, s * a.z }; } 61 | inline Vec3f operator/ (float s, Vec3f a) { return { s / a.x, s / a.y, s / a.z }; } 62 | inline Vec3f operator/ (Vec3f a, float s) { return { a.x / s , a.y / s , a.z / s }; } 63 | inline Vec3f& operator+=(Vec3f& a, Vec3f b) { a.x += b.x; a.y += b.y; a.z += b.z; return a; } 64 | inline Vec3f& operator-=(Vec3f& a, Vec3f b) { a.x -= b.x; a.y -= b.y; a.z -= b.z; return a; } 65 | inline Vec3f& operator*=(Vec3f& a, Vec3f b) { a.x *= b.x; a.y *= b.y; a.z *= b.z; return a; } 66 | inline Vec3f& operator/=(Vec3f& a, Vec3f b) { a.x /= b.x; a.y /= b.y; a.z /= b.z; return a; } 67 | inline Vec3f& operator*=(Vec3f& a, float s) { a.x *= s ; a.y *= s ; a.z *= s ; return a; } 68 | inline Vec3f& operator/=(Vec3f& a, float s) { a.x /= s ; a.y /= s ; a.z /= s ; return a; } 69 | 70 | inline Vec4f operator+ (Vec4f v) { return { +v.x, +v.y, +v.z, +v.w }; } 71 | inline Vec4f operator- (Vec4f v) { return { -v.x, -v.y, -v.z, -v.w }; } 72 | inline Vec4f operator+ (Vec4f a, Vec4f b) { return { a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w }; } 73 | inline Vec4f operator- (Vec4f a, Vec4f b) { return { a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w }; } 74 | inline Vec4f operator* (Vec4f a, Vec4f b) { return { a.x * b.x, a.y * b.y, a.z * b.z, a.w * b.w }; } 75 | inline Vec4f operator/ (Vec4f a, Vec4f b) { return { a.x / b.x, a.y / b.y, a.z / b.z, a.w / b.w }; } 76 | inline Vec4f operator* (float s, Vec4f a) { return { s * a.x, s * a.y, s * a.z, s * a.w }; } 77 | inline Vec4f operator* (Vec4f a, float s) { return { s * a.x, s * a.y, s * a.z, s * a.w }; } 78 | inline Vec4f operator/ (float s, Vec4f a) { return { s / a.x, s / a.y, s / a.z, s / a.w }; } 79 | inline Vec4f operator/ (Vec4f a, float s) { return { s / a.x, s / a.y, s / a.z, s / a.w }; } 80 | inline Vec4f& operator+=(Vec4f& a, Vec4f b) { a.x += b.x; a.y += b.y; a.z += b.z; a.w += b.w; return a; } 81 | inline Vec4f& operator-=(Vec4f& a, Vec4f b) { a.x -= b.x; a.y -= b.y; a.z -= b.z; a.w -= b.w; return a; } 82 | inline Vec4f& operator*=(Vec4f& a, Vec4f b) { a.x *= b.x; a.y *= b.y; a.z *= b.z; a.w *= b.w; return a; } 83 | inline Vec4f& operator/=(Vec4f& a, Vec4f b) { a.x /= b.x; a.y /= b.y; a.z /= b.z; a.w /= b.w; return a; } 84 | inline Vec4f& operator*=(Vec4f& a, float s) { a.x *= s ; a.y *= s ; a.z *= s ; a.w *= s ; return a; } 85 | inline Vec4f& operator/=(Vec4f& a, float s) { a.x /= s ; a.y /= s ; a.z /= s ; a.w /= s ; return a; } 86 | 87 | inline float dot (Vec2f a, Vec2f b) { return a.x * b.x + a.y * b.y; } 88 | inline float len (Vec2f v) { return sqrtf(v.x * v.x + v.y * v.y); } 89 | inline float sqrlen (Vec2f v) { return v.x * v.x + v.y * v.y; } 90 | inline Vec2f norm (Vec2f v) { return (1.0f / len(v)) * v; } 91 | inline Vec2f norm_safe(Vec2f v) { return (1.0f / (len(v) + 1e-8f)) * v; } 92 | inline Vec2f abs (Vec2f v) { return { fabsf(v.x), fabsf(v.y) }; } 93 | inline Vec2f cross (Vec2f v) { return { -v.y, v.x }; } 94 | 95 | inline float dot (Vec3f a, Vec3f b) { return a.x * b.x + a.y * b.y + a.z * b.z; } 96 | inline float len (Vec3f v) { return sqrtf(v.x * v.x + v.y * v.y + v.z * v.z); } 97 | inline float sqrlen (Vec3f v) { return v.x * v.x + v.y * v.y + v.z * v.z; } 98 | inline Vec3f norm (Vec3f v) { return (1.0f / len(v)) * v; } 99 | inline Vec3f norm_safe(Vec3f v) { return (1.0f / (len(v) + 1e-8f)) * v; } 100 | inline Vec3f abs (Vec3f v) { return { fabsf(v.x), fabsf(v.y), fabsf(v.z) }; } 101 | inline Vec3f cross (Vec3f a, Vec3f b) { return { a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x }; } 102 | inline Vec3f cross_z (Vec3f v) { return { v.y, -v.x, 0.0f }; } 103 | 104 | inline float dot (Vec4f a, Vec4f b) { return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w; } 105 | inline float len (Vec4f v) { return sqrtf(v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w); } 106 | inline float sqrlen (Vec4f v) { return v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w; } 107 | inline Vec4f norm (Vec4f v) { return (1.0f / len(v)) * v; } 108 | inline Vec4f norm_safe(Vec4f v) { return (1.0f / (len(v) + 1e-8f)) * v; } 109 | inline Vec4f abs (Vec4f v) { return { fabsf(v.x), fabsf(v.y), fabsf(v.z), fabsf(v.w) }; } 110 | 111 | inline float lerp(float a, float b, float t) { return (1.0f - t) * a + t * b; } 112 | inline Vec2f lerp(Vec2f a, Vec2f b, float t) { return (1.0f - t) * a + t * b; } 113 | inline Vec3f lerp(Vec3f a, Vec3f b, float t) { return (1.0f - t) * a + t * b; } 114 | inline Vec4f lerp(Vec4f a, Vec4f b, float t) { return (1.0f - t) * a + t * b; } 115 | 116 | // Necessary utilities pulled from https://github.com/andrewwillmott/vl 117 | typedef Vec4f Quatf; 118 | 119 | Quatf QuatMult (const Quatf& a, const Quatf& b); // Concatenate quaternions: the result represents applying 'a' then 'b'. 120 | Vec3f QuatApply(const Vec3f& p, const Quatf& q); // Transform point p by applying quaternion q 121 | Quatf QuatConj (const Quatf& q); // Quaternion conjugate. if len(q) = 1, this is also the inverse. 122 | 123 | Quatf SLerp(const Quatf& q1, const Quatf& q2, float s); // Return spherical interpolation between q1 and q2 124 | 125 | Vec3f LogUnit3(const Quatf& q); // LogUnit variant that omits the last (zero) component 126 | Quatf ExpUnit3(const Vec3f& lq); // ExpUnit variant that omits the last (zero) component 127 | Vec3f QuatDiff3(const Quatf& a, const Quatf& b); // Returns LogUnit3(MakeQuat(a, b)) -- useful for rotational velocity, and other log/tangent-space deltas 128 | 129 | inline Quatf QuatMult(const Quatf& a, const Quatf& b) 130 | { 131 | Quatf result; 132 | result.x = + a.w * b.x + a.z * b.y - a.y * b.z + a.x * b.w; 133 | result.y = - a.z * b.x + a.w * b.y + a.x * b.z + a.y * b.w; 134 | result.z = + a.y * b.x - a.x * b.y + a.w * b.z + a.z * b.w; 135 | result.w = - a.x * b.x - a.y * b.y - a.z * b.z + a.w * b.w; 136 | return result; 137 | } 138 | 139 | inline const Vec3f& xyz(const Quatf& q) { return (const Vec3f&) q; } 140 | inline Vec3f& xyz( Quatf& q) { return ( Vec3f&) q; } 141 | 142 | inline Vec3f QuatApply(const Vec3f& p, const Quatf& q) 143 | { 144 | Vec3f b0 = cross(xyz(q), p); 145 | Vec3f b1 = cross(xyz(q), b0); 146 | return p + 2 * (b0 * q.w + b1); 147 | } 148 | 149 | inline Quatf QuatConj(const Quatf& q) 150 | { 151 | return Quatf(-q.x, -q.y, -q.z, q.w); 152 | } 153 | 154 | inline Quatf SLerp(const Quatf& q1, const Quatf& q2, float s) 155 | { 156 | float cosHalfTheta = dot(q1, q2); 157 | 158 | if (abs(cosHalfTheta) >= float(0.99999)) 159 | return q1; 160 | 161 | float sinHalfTheta = sqrt(float(1) - cosHalfTheta * cosHalfTheta); 162 | 163 | if (sinHalfTheta < float(1e-5)) 164 | return float(0.5) * (q1 + q2); 165 | 166 | float halfTheta = atan2f(sinHalfTheta, cosHalfTheta); 167 | 168 | float t = float(1) - s; 169 | float ratio1 = sinf(t * halfTheta) / sinHalfTheta; 170 | float ratio2 = sinf(s * halfTheta) / sinHalfTheta; 171 | 172 | return ratio1 * q1 + ratio2 * q2; 173 | } 174 | 175 | inline Vec3f LogUnit3(const Quatf& q) 176 | { 177 | float s = len(xyz(q)); 178 | float c = q.w; 179 | return (atan2f(s, c) / (s + float(1e-8))) * xyz(q); 180 | } 181 | 182 | inline Quatf ExpUnit3(const Vec3f& lq) 183 | { 184 | float theta = len(lq); 185 | Quatf q; 186 | xyz(q) = lq * (sinf(theta) / (theta + float(1e-8))); 187 | q.w = cosf(theta); 188 | return q; 189 | } 190 | 191 | inline Vec3f QuatDiff3(const Quatf& a, const Quatf& b) 192 | { 193 | Vec3f v; 194 | v.x = + a.w * b.x - a.z * b.y + a.y * b.z - a.x * b.w; 195 | v.y = + a.z * b.x + a.w * b.y - a.x * b.z - a.y * b.w; 196 | v.z = - a.y * b.x + a.x * b.y + a.w * b.z - a.z * b.w; 197 | 198 | float c = dot(a, b); 199 | float s = len(v); 200 | 201 | v *= (atan2f(s, c) / (s + float(1e-8))); 202 | 203 | return v; 204 | } 205 | 206 | #endif 207 | -------------------------------------------------------------------------------- /RotSplines.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // RotSplines.cpp 3 | // 4 | // Cubic spline utilities for rotations 5 | // 6 | // Andrew Willmott 7 | // 8 | 9 | #include "RotSplines.hpp" 10 | 11 | using namespace SL; 12 | 13 | namespace 14 | { 15 | inline float ConstrainAngle(float ra, float rb) 16 | { 17 | float d = (rb - ra) * (1.0f / vlf_twoPi); 18 | float id = floorf(d + 0.5f); 19 | return ra + (d - id) * vlf_twoPi; 20 | } 21 | 22 | inline RotSpline2 RotSplineFromAngles(const float a[], int i0, int i1, int i2, int i3, float s) 23 | { 24 | float pb1 = a[i1] + s * (a[i2] - a[i0]); 25 | float pb2 = a[i2] - s * (a[i3] - a[i1]); 26 | 27 | return BezierSpline(a[i1], pb1, pb2, a[i2]); 28 | } 29 | } 30 | 31 | int SL::RotSplinesFromAngles(int numAngles, const float ai[], RotSpline2 splines[], float tension, size_t stride) 32 | { 33 | SL_ASSERT(numAngles >= 0); 34 | 35 | float s = (1.0f - tension) * (1.0f / 6.0f); // 1/2 for averaging * 1/3 for v scale 36 | 37 | float local[4]; 38 | int ln = vl_min(numAngles, 4); 39 | 40 | for (int i = 0; i < ln; i++) 41 | { 42 | local[i] = *ai; 43 | (char*&) ai += stride; 44 | } 45 | 46 | for (int i = 1; i < ln; i++) 47 | local[i] = ConstrainAngle(local[i - 1], local[i]); 48 | 49 | switch (numAngles) 50 | { 51 | case 0: return 0; 52 | case 1: *splines = RotSplineFromAngles(local, 0, 0, 0, 0, s); return 1; 53 | case 2: *splines = RotSplineFromAngles(local, 0, 0, 1, 1, s); return 1; 54 | } 55 | 56 | *splines++ = RotSplineFromAngles(local, 0, 0, 1, 2, s); 57 | int base = 0; 58 | int i0 = 0, i1 = 1, i2 = 2, i3 = 3; 59 | 60 | while (true) 61 | { 62 | *splines++ = RotSplineFromAngles(local, i0, i1, i2, i3, s); 63 | 64 | i0 = i1; i1 = i2; i2 = i3; i3 = (base++ & 3); 65 | 66 | if (base == numAngles - 3) 67 | break; 68 | 69 | local[i3] = ConstrainAngle(local[i2], *ai); 70 | (char*&) ai += stride; 71 | } 72 | 73 | *splines++ = RotSplineFromAngles(local, i0, i1, i2, i2, s); 74 | 75 | return numAngles - 1; 76 | } 77 | 78 | int SL::RotSplinesFromDirs(int numAngles, const Vec2f d[], RotSpline2 splines[], float tension, size_t stride) 79 | { 80 | SL_ASSERT(numAngles >= 0); 81 | 82 | float s = (1.0f - tension) * (1.0f / 6.0f); // 1/2 for averaging * 1/3 for v scale 83 | 84 | float local[4]; 85 | int ln = vl_min(numAngles, 4); 86 | 87 | for (int i = 0; i < ln; i++) 88 | { 89 | local[i] = atan2f(d->y, d->x); 90 | (char*&) d += stride; 91 | } 92 | 93 | for (int i = 1; i < ln; i++) 94 | local[i] = ConstrainAngle(local[i - 1], local[i]); 95 | 96 | switch (numAngles) 97 | { 98 | case 0: return 0; 99 | case 1: *splines = RotSplineFromAngles(local, 0, 0, 0, 0, s); return 1; 100 | case 2: *splines = RotSplineFromAngles(local, 0, 0, 1, 1, s); return 1; 101 | } 102 | 103 | *splines++ = RotSplineFromAngles(local, 0, 0, 1, 2, s); 104 | int base = 0; 105 | int i0 = 0, i1 = 1, i2 = 2, i3 = 3; 106 | 107 | while (true) 108 | { 109 | *splines++ = RotSplineFromAngles(local, i0, i1, i2, i3, s); 110 | 111 | i0 = i1; i1 = i2; i2 = i3; i3 = (base++ & 3); 112 | 113 | if (base == numAngles - 3) 114 | break; 115 | 116 | local[i3] = ConstrainAngle(local[i2], atan2f(d->y, d->x)); 117 | (char*&) d += stride; 118 | } 119 | 120 | *splines++ = RotSplineFromAngles(local, i0, i1, i2, i2, s); 121 | 122 | return numAngles - 1; 123 | } 124 | 125 | // 126 | // Note: if adapting this code for use with an alternative quaternion library, 127 | // be aware that to make the code more readable, the quaternion multiplication 128 | // routines here work left-to-right. I.e., QuatMult(a, b) = apply rotation a 129 | // and then rotation b. QuatApply() is the standard transformation of a 3d 130 | // point by a vector, which has no such ordering issues. 131 | // 132 | 133 | namespace 134 | { 135 | // b = 1, 1 - (1 - t)^3, 3t^2 - 2t^3, t^3 136 | // b' = 0, 3(1 - t)^2, 6t - 6t^2, 3 t^2 137 | // b'' = 0, -6(1 - t) , 6 - 12t , 6 t 138 | 139 | inline Vec3f BezierRotWeights(float t) 140 | { 141 | float t2 = sqr(t); 142 | float t3 = t2 * t; 143 | float it = 1 - t; 144 | float it3 = it * it * it; 145 | 146 | return { 1 - it3, 3 * t2 - 2 * t3, t3 }; 147 | } 148 | 149 | inline Vec3f BezierRotWeightsD1(float t) 150 | { 151 | float t2 = sqr(t); 152 | float it = 1.0f - t; 153 | float it2 = sqr(it); 154 | 155 | return { 3 * it2, 6 * t * it, 3 * t2 }; 156 | } 157 | 158 | inline Vec3f BezierRotWeightsD2(float t) 159 | { 160 | return { -6 + 6 * t, 6 - 12 * t, 6 * t }; 161 | } 162 | } 163 | 164 | RotSpline3 SL::BezierRotSpline(Quatf q0, Quatf q1, Quatf q2, Quatf q3) 165 | { 166 | return RotSpline3 167 | { 168 | q0, 169 | QuatDiff3(q0, q1), 170 | QuatDiff3(q1, q2), 171 | QuatDiff3(q2, q3) 172 | }; 173 | } 174 | 175 | #if 0 176 | RotSpline3 SL::HermiteRotSplineRef(Quatf q0, Quatf q1, Vec3f w0, Vec3f w1) 177 | { 178 | w0 *= float(1.0 / 3.0); 179 | w1 *= float(1.0 / 3.0); 180 | 181 | // Easier to understand Bezier form, following 182 | // qm0 = q0 + w0 / 3 183 | // qm1 = q0 - w1 / 3 184 | Quatf qm0 = QuatMult(q0, ExpUnit3(+w0)); 185 | Quatf qm1 = QuatMult(q1, ExpUnit3(-w1)); // q1 exp(w1)-1 = q1 exp(-w1) 186 | return BezierRotSpline(q0, qm0, qm1, q1); 187 | } 188 | #endif 189 | 190 | RotSpline3 SL::HermiteRotSpline(Quatf q0, Quatf q1, Vec3f w0, Vec3f w1) 191 | { 192 | w0 *= float(1.0 / 3.0); 193 | w1 *= float(1.0 / 3.0); 194 | 195 | // More efficient: find middle delta 'wm' directly and re-use w0/w1 196 | // q0 e^w0 e^wm e^w1 = q1 -> e^wm = e^-w0 q0-1 q1 e^-w1 197 | Quatf qm; 198 | qm = ExpUnit3(-w0); 199 | qm = QuatMult(qm, QuatMult(QuatConj(q0), q1)); 200 | qm = QuatMult(qm, ExpUnit3(-w1)); 201 | 202 | Vec3f wm = LogUnit3(qm); 203 | 204 | return RotSpline3{ q0, w0, wm, w1 }; 205 | } 206 | 207 | RotSpline3 SL::LineRotSpline(Quatf q0, Quatf q1) 208 | { 209 | RotSpline3 rs; 210 | rs.q0 = q0; 211 | 212 | Vec3f w = QuatDiff3(q0, q1) * (1.0f / 3.0f); 213 | rs.w1 = w; 214 | rs.w2 = w; 215 | rs.w3 = w; 216 | 217 | return rs; 218 | } 219 | 220 | Quatf SL::Rotation0(const RotSpline3& rs) 221 | { 222 | return rs.q0; 223 | } 224 | 225 | Quatf SL::Rotation1(const RotSpline3& rs) 226 | { 227 | Quatf r = rs.q0; 228 | r = QuatMult(r, ExpUnit3(rs.w1)); 229 | r = QuatMult(r, ExpUnit3(rs.w2)); 230 | r = QuatMult(r, ExpUnit3(rs.w3)); 231 | return r; 232 | } 233 | 234 | Vec3f SL::RotVelocity0(const RotSpline3& rs) 235 | { 236 | return rs.w1 * 3; 237 | } 238 | 239 | Vec3f SL::RotVelocity1(const RotSpline3& rs) 240 | { 241 | return rs.w3 * 3; 242 | } 243 | 244 | Quatf SL::Rotation(const RotSpline3& rs, float t) 245 | { 246 | // Note: in contrast to the geometric construction in Shoemake's approaches, 247 | // this routine uses Kim/Kim/Shin's general algebraic construction scheme 248 | // for unit quaternion splines: 249 | // 250 | // https://www.semanticscholar.org/paper/A-general-construction-scheme-for-unit-quaternion-Kim-Kim/95336585fc493e66ed79132d2fefd4d157f8cc49 251 | // 252 | // The basic idea is to use the exponential map to do the basis function 253 | // interpolation in axis/angle space. If a unit quaternion q is written as 254 | // [n sin(w), cos(w)], then log(q) = [n w, 0], and the exponent of that gets 255 | // you back to the original quaternion as you might expect. 256 | // 257 | // In 2D, the equivalent of this, as implemented above, is to operate 258 | // directly on angles, and then at the end convert the result to an 259 | // orientation vector via sin and cos (the 2D quaternion equivalent), and 260 | // hence to a matrix. You may be familiar with the corresponding exp/log 261 | // relationship: exp(i w) = i sin(w), cos(w). 262 | // 263 | // In 3D, because rotations don't commute, the addition part of this 264 | // approach no longer works, because exp(a) exp(b) != exp(a + b). (There is 265 | // a formula for exp(a) exp(b) in terms of AB - BA differences, but it does 266 | // not converge well in the case of quaternions.) We can still apply the 't' 267 | // part of 'a + t(b - a)' (say) in log(q) space, but have to transform the 268 | // result back to quaternion space to concatenate the parts. Hence the 269 | // classic log form of SLerp isn't 270 | // 271 | // q = exp(log(a) + t (log(b) - log(a))) 272 | // 273 | // but rather 274 | // 275 | // q = a exp(t log(b-1 a)) 276 | // 277 | // For cubic splines, we take the same approach, but now we must concatenate 278 | // four of these terms (one for each basis weight/control point), and, 279 | // because we are not free to re-arrange terms due to commutivity, the 280 | // standard Bezier weights must be recast in cumulative form. 281 | // 282 | // Conceptually this is a little more complex than the geometric 283 | // formulations that utilise nested slerps, but with the right choice of 284 | // tangents (namely in/out rotation velocities) the results can be C2 285 | // continuous, and, the log/exp framework makes derivatives straightforward. 286 | 287 | // Right, let's do this thing. The code itself is pretty straight-forward. 288 | Vec3f b = BezierRotWeights(t); // cumulative weights rather than individual BezierWeights(). First implicit term is thus always 1. 289 | 290 | Quatf r = rs.q0; 291 | r = QuatMult(r, ExpUnit3(b.x * rs.w1)); // Basically three quat multiplies + 3 sin/cos + 3v*. No acos/atan. 292 | r = QuatMult(r, ExpUnit3(b.y * rs.w2)); // Shoemake original approach is 6 slerps. SQuad is 3 slerps. 293 | r = QuatMult(r, ExpUnit3(b.z * rs.w3)); 294 | 295 | return r; 296 | } 297 | 298 | // #define USE_REFERENCE 299 | 300 | namespace 301 | { 302 | template inline Quatf QuatMult(const Quatf& q0, const Quatf& q1, T... args) 303 | { 304 | return QuatMult(QuatMult(q0, q1), args...); 305 | } 306 | 307 | #ifdef USE_REFERENCE 308 | Vec3f RotVelocityRef(const RotSpline3& rs, float t) 309 | { 310 | // Everything written out for clarity. We're implementing: 311 | // q = q0 exp(w1 b1) exp(w2 b2) exp(w3 b3) 312 | // q' = 313 | // q0 exp(w1 b1) (w1 b'1) exp(w2 b2) exp(w3 b3) 314 | // + q0 exp(w1 b1) exp(w2 b2) (w2 b'2) exp(w3 b3) 315 | // + q0 exp(w1 b1) exp(w2 b2) exp(w3 b3) (w3 b'3) 316 | // 317 | // The above looks more complex than the linear form that can be used 318 | // in the various other Velocity routines, but it's just the chain 319 | // rule applied to the multiplied terms, vs the additions elsewhere. 320 | 321 | Vec3f b0 = BezierRotWeights (t); 322 | Vec3f b1 = BezierRotWeightsD1(t); 323 | 324 | Quatf qd1(ExpUnit3(rs.w1 * b0.x)); 325 | Quatf qd2(ExpUnit3(rs.w2 * b0.y)); 326 | Quatf qd3(ExpUnit3(rs.w3 * b0.z)); 327 | 328 | Quatf qv1; xyz(qv1) = rs.w1 * b1.x; qv1.w = 0; 329 | Quatf qv2; xyz(qv2) = rs.w2 * b1.y; qv2.w = 0; 330 | Quatf qv3; xyz(qv3) = rs.w3 * b1.z; qv3.w = 0; 331 | 332 | Quatf qv; 333 | qv = QuatMult(rs.q0, qd1, qv1, qd2, qd3); 334 | qv += QuatMult(rs.q0, qd1, qd2, qv2, qd3); 335 | qv += QuatMult(rs.q0, qd1, qd2, qd3, qv3); 336 | 337 | // At this point we have q'(t) = q(t) w'(t), analogous to (e^iw)' = w' e^iw 338 | // so we need to invert out the q(t) part to get w'(t) 339 | Quatf q = QuatMult(rs.q0, qd1, qd2, qd3); 340 | Quatf w = QuatMult(QuatConj(q), qv); 341 | 342 | return xyz(w); 343 | } 344 | #endif 345 | } 346 | 347 | Vec3f SL::RotVelocity(const RotSpline3& rs, float t) 348 | { 349 | // This is a much-simplified version of RotVelocityRef with terms cancelled out. 350 | // 1 QuatMult, 2 QuatApply, 2 ExpUnit3=sincos, vs Rotation = 3 QM, 3 EU 351 | Vec3f b = BezierRotWeights (t); 352 | Vec3f v = BezierRotWeightsD1(t); 353 | 354 | Quatf qd2 = ExpUnit3(rs.w2 * b.y); 355 | Quatf qd3 = ExpUnit3(rs.w3 * b.z); 356 | 357 | Quatf qd23 = QuatMult(qd2, qd3); 358 | 359 | Vec3f wv1(rs.w1 * v.x); 360 | Vec3f wv2(rs.w2 * v.y); 361 | Vec3f wv3(rs.w3 * v.z); 362 | 363 | Vec3f w; 364 | w = QuatApply(wv1, qd23); 365 | w += QuatApply(wv2, qd3 ); 366 | w += wv3; 367 | 368 | #ifdef USE_REFERENCE 369 | SL_ASSERT(len(w - RotVelocityRef(rs, t)) < 1e-5f); 370 | #endif 371 | 372 | return w; 373 | } 374 | 375 | namespace 376 | { 377 | // Quat/Vector Quat multiply 378 | inline Quatf QuatMult(const Quatf& a, const Vec3f& b) 379 | { 380 | Quatf q; 381 | xyz(q) = cross(xyz(a), b) + a.w * b; 382 | q.w = -dot(xyz(a), b); 383 | return q; 384 | } 385 | } 386 | 387 | Quatf SL::RotAcceleration(const RotSpline3& rs, float t) 388 | { 389 | /* 390 | A01 = exp(w1 b1) 391 | A02 = exp(w2 b2) 392 | A03 = exp(w3 b3) 393 | 394 | A11 = A01' = A01 (w1 b'1) 395 | A12 = A02' = A02 (w2 b'2) 396 | A13 = A03' = A03 (w3 b'3) 397 | 398 | A21 = A11' = A01 (w1 b''1) + A11 (w1 b'1) 399 | A22 = A12' = A02 (w2 b''1) + A12 (w2 b'1) 400 | A23 = A13' = A03 (w3 b''1) + A13 (w3 b'1) 401 | 402 | q' = (A11 A02 A03 + A01 A12 A03 + A01 A02 A13) 403 | 404 | q'' = (A21 A02 A03 + A01 A22 A03 + A01 A02 A23) 405 | + 2 * (A11 A12 A03 + A11 A02 A13 + A01 A12 A13) 406 | */ 407 | 408 | Vec3f b0 = BezierRotWeights (t); 409 | Vec3f b1 = BezierRotWeightsD1(t); 410 | Vec3f b2 = BezierRotWeightsD2(t); 411 | 412 | Quatf a01(ExpUnit3(rs.w1 * b0.x)); 413 | Quatf a02(ExpUnit3(rs.w2 * b0.y)); 414 | Quatf a03(ExpUnit3(rs.w3 * b0.z)); 415 | 416 | Vec3f wv1(rs.w1 * b1.x); 417 | Vec3f wv2(rs.w2 * b1.y); 418 | Vec3f wv3(rs.w3 * b1.z); 419 | 420 | Vec3f wa1(rs.w1 * b2.x); 421 | Vec3f wa2(rs.w2 * b2.y); 422 | Vec3f wa3(rs.w3 * b2.z); 423 | 424 | Quatf a11 = QuatMult(a01, wv1); 425 | Quatf a12 = QuatMult(a02, wv2); 426 | Quatf a13 = QuatMult(a03, wv3); 427 | 428 | Quatf a21 = QuatMult(a11, wv1) + QuatMult(a01, wa1); // a01 (wv1 wv1 + wa1) 429 | Quatf a22 = QuatMult(a12, wv2) + QuatMult(a02, wa2); 430 | Quatf a23 = QuatMult(a13, wv3) + QuatMult(a03, wa3); 431 | 432 | Quatf wd = QuatMult(a01, a02, a03); 433 | 434 | Quatf qa1; 435 | qa1 = QuatMult(a21, a02, a03); 436 | qa1 += QuatMult(a01, a22, a03); 437 | qa1 += QuatMult(a01, a02, a23); 438 | 439 | Quatf qa2; 440 | qa2 = QuatMult(a11, a12, a03); 441 | qa2 += QuatMult(a11, a02, a13); 442 | qa2 += QuatMult(a01, a12, a13); 443 | 444 | Quatf qa = qa1 + 2 * qa2; 445 | Quatf wa = QuatMult(QuatConj(wd), qa); 446 | 447 | return wa; 448 | } 449 | 450 | Quatf SL::Rotation(Quatf q0, Vec3f w1, Vec3f w2, Vec3f w3, float t) 451 | { 452 | Vec3f b = BezierRotWeights(t); 453 | 454 | Quatf r = q0; 455 | r = QuatMult(r, ExpUnit3(b.x * w1)); 456 | r = QuatMult(r, ExpUnit3(b.y * w2)); 457 | r = QuatMult(r, ExpUnit3(b.z * w3)); 458 | return r; 459 | } 460 | 461 | Quatf SL::Rotation(Quatf q0, Quatf q1, Quatf q2, Quatf q3, float t) 462 | { 463 | Vec3f w1 = QuatDiff3(q0, q1); 464 | Vec3f w2 = QuatDiff3(q1, q2); 465 | Vec3f w3 = QuatDiff3(q2, q3); 466 | 467 | return Rotation(q0, w1, w2, w3, t); 468 | } 469 | 470 | Quatf SL::RotationShoemake(const RotSpline3& rs, float t) 471 | { 472 | Quatf q0 = rs.q0; 473 | 474 | // We're saving the acos here, but only in the first round. 475 | Quatf q01 = QuatMult(q0, ExpUnit3(rs.w1 * t)); 476 | Quatf q1 = QuatMult(q0, ExpUnit3(rs.w1)); 477 | Quatf q12 = QuatMult(q1, ExpUnit3(rs.w2 * t)); 478 | Quatf q2 = QuatMult(q1, ExpUnit3(rs.w2)); 479 | Quatf q23 = QuatMult(q2, ExpUnit3(rs.w3 * t)); 480 | 481 | Quatf q012 = SLerp(q01, q12, t); 482 | Quatf q123 = SLerp(q12, q23, t); 483 | 484 | Quatf q0123 = SLerp(q012, q123, t); 485 | 486 | return q0123; 487 | } 488 | 489 | Quatf SL::RotationShoemake(Quatf q0, Quatf q1, Quatf q2, Quatf q3, float t) 490 | { 491 | // Basically, just De Casteljau's algorithm using slerps rather than linear interpolation. 492 | Quatf q01 = SLerp(q0, q1, t); 493 | Quatf q12 = SLerp(q1, q2, t); 494 | Quatf q23 = SLerp(q2, q3, t); 495 | 496 | Quatf q012 = SLerp(q01, q12, t); 497 | Quatf q123 = SLerp(q12, q23, t); 498 | 499 | Quatf q0123 = SLerp(q012, q123, t); 500 | 501 | return q0123; 502 | } 503 | 504 | Quatf SL::RotationSQuad(const RotSpline3& rs, float t) 505 | { 506 | // Cheaper version where you slerp the end points and the interior points, 507 | // then quadratically blend between the end point result and the interior 508 | // result, such that you get the interior point in the middle and exterior 509 | // points at either end. *waves hands*. 510 | // 511 | // RotSpline3 is not the best format here, q0, w123, q1, w2 would be better, 512 | // but this is just for cross-checking. Use the standalone variant if this 513 | // is being used in anger. 514 | Quatf q0 = rs.q0; 515 | 516 | Quatf q1 = QuatMult(q0, ExpUnit3(rs.w1)); 517 | Quatf q12 = QuatMult(q1, ExpUnit3(rs.w2 * t)); 518 | 519 | Quatf q3 = QuatMult(q1, QuatMult(ExpUnit3(rs.w2), ExpUnit3(rs.w3))); // :| 520 | Quatf q03 = SLerp(q0, q3, t); 521 | 522 | Quatf q0123 = SLerp(q03, q12, 2 * t * (1 - t)); 523 | 524 | return q0123; 525 | } 526 | 527 | Quatf SL::RotationSQuad(Quatf q0, Quatf q1, Quatf q2, Quatf q3, float t) 528 | { 529 | Quatf q03 = SLerp(q0, q3, t); 530 | Quatf q12 = SLerp(q1, q2, t); 531 | 532 | Quatf q0123 = SLerp(q03, q12, 2 * t * (1 - t)); 533 | 534 | return q0123; 535 | } 536 | 537 | void SL::Split(const RotSpline3& rs, float t, RotSpline3* spline0, RotSpline3* spline1) 538 | { 539 | Quatf qm = Rotation(rs, t); 540 | Vec3f wm = RotVelocity(rs, t); 541 | 542 | Quatf q0 = Rotation0(rs); 543 | Quatf q1 = Rotation1(rs); 544 | 545 | Vec3f w0 = RotVelocity0(rs); 546 | Vec3f w1 = RotVelocity1(rs); 547 | 548 | *spline0 = HermiteRotSpline(q0, qm, w0, wm); 549 | *spline1 = HermiteRotSpline(qm, q1, wm, w1); 550 | } 551 | 552 | bool SL::Join(const RotSpline3& rs0, const RotSpline3& rs1, RotSpline3* rs) 553 | { 554 | if (rs0.w3 != rs0.w1) // early out 555 | return false; 556 | 557 | *rs = HermiteRotSpline(Rotation0(rs0), Rotation1(rs1), RotVelocity0(rs0), RotVelocity1(rs1)); 558 | return true; 559 | } 560 | 561 | namespace 562 | { 563 | inline Quatf ConstrainQuat(const Quatf& q1, const Quatf& q2) 564 | { 565 | return (dot(q1, q2) < 0) ? -q2 : q2; 566 | } 567 | 568 | inline RotSpline3 RotSplineFromQuats(const Quatf q[], int i0, int i1, int i2, int i3, float s) 569 | { 570 | Vec3f wb1 = s * QuatDiff3(q[i0], q[i2]); 571 | Vec3f wb2 = s * QuatDiff3(q[i1], q[i3]); 572 | 573 | return HermiteRotSpline(q[i1], q[i2], wb1, wb2); 574 | } 575 | } 576 | 577 | int SL::RotSplinesFromQuats(int numQuats, const Quatf q[], RotSpline3 splines[], float tension, size_t stride) 578 | { 579 | SL_ASSERT(numQuats >= 0); 580 | 581 | float s = (1.0f - tension) * (1.0f / 6.0f); // 1/2 for averaging * 1/3 for v scale 582 | 583 | Quatf local[4]; 584 | int ln = vl_min(numQuats, 4); 585 | 586 | for (int i = 0; i < ln; i++) 587 | { 588 | local[i] = *q; 589 | (char*&) q += stride; 590 | } 591 | 592 | for (int i = 1; i < ln; i++) 593 | local[i] = ConstrainQuat(local[i - 1], local[i]); 594 | 595 | switch (numQuats) 596 | { 597 | case 0: return 0; 598 | case 1: *splines = RotSplineFromQuats(local, 0, 0, 0, 0, s); return 1; 599 | case 2: *splines = RotSplineFromQuats(local, 0, 0, 1, 1, s); return 1; 600 | } 601 | 602 | *splines++ = RotSplineFromQuats(local, 0, 0, 1, 2, s); 603 | int base = 0; 604 | int i0 = 0, i1 = 1, i2 = 2, i3 = 3; 605 | 606 | while (true) 607 | { 608 | *splines++ = RotSplineFromQuats(local, i0, i1, i2, i3, s); 609 | 610 | i0 = i1; i1 = i2; i2 = i3; i3 = (base++ & 3); 611 | 612 | if (base == numQuats - 3) 613 | break; 614 | 615 | local[i3] = ConstrainQuat(local[i2], *q); 616 | (char*&) q += stride; 617 | } 618 | 619 | *splines++ = RotSplineFromQuats(local, i0, i1, i2, i2, s); 620 | 621 | return numQuats - 1; 622 | } 623 | -------------------------------------------------------------------------------- /extra/SplinesDraw.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // SplinesDraw.cpp 3 | // 4 | // Draw support for splines 5 | // 6 | // Andrew Willmott 7 | // 8 | 9 | #include "SplinesDraw.hpp" 10 | 11 | #include "Draw.hpp" 12 | #include "MinMax.hpp" 13 | #include "Transform2.hpp" 14 | #include "Transform3.hpp" 15 | 16 | using namespace SL; 17 | 18 | namespace 19 | { 20 | inline float Fract(float x) 21 | { 22 | return x - floorf(x); 23 | } 24 | 25 | Colour Fract(const Colour& c) 26 | { 27 | return { Fract(c[0]), Fract(c[1]), Fract(c[2]) }; 28 | } 29 | 30 | Colour SplineColour(const Spline2& spline, float t, SplineDrawMode mode, float scale, int i) 31 | { 32 | switch (mode) 33 | { 34 | case kSplineDrawModeNone: 35 | scale = 0.0f; 36 | break; 37 | 38 | case kSplineDrawModeT: 39 | scale *= t; 40 | break; 41 | 42 | case kSplineDrawModePosition: 43 | return Fract(scale * Vec3f(Position(spline, t), 0.0)); 44 | 45 | case kSplineDrawModeVelocity: 46 | scale *= len(Velocity(spline, t)) * 0.001f; 47 | break; 48 | 49 | case kSplineDrawModeAcceleration: 50 | scale *= len(Acceleration(spline, t)) * 0.01f; 51 | 52 | case kSplineDrawModeCurvature: 53 | scale *= Curvature(spline, t) * 100.0f; 54 | break; 55 | 56 | case kSplineDrawModeLength: 57 | if (t >= 0.5f) 58 | scale *= Length(spline, t, 1.0f) * 0.01f; 59 | else 60 | scale *= Length(spline, 0.0f, t) * 0.01f; 61 | break; 62 | 63 | case kSplineDrawModeLine: 64 | return LabelColour(i); 65 | break; 66 | 67 | case kNumSplineDrawModes: 68 | SL_ERROR("bad enum"); 69 | } 70 | 71 | scale = ClampUnit(scale); 72 | float hue = lerp(kHueGreen, kHueRed + 360.0f, scale); // move through the blues to red 73 | 74 | return HSVColour(hue, 1.0f, 1.0f); 75 | } 76 | } 77 | 78 | void SL::DrawSplineLines(int numLines, Vec2f lines[][2], const Spline2&, int i, int n, float[][2], void* context) 79 | { 80 | reinterpret_cast(context)->DrawLines(2 * numLines, lines[0]); 81 | } 82 | 83 | void SL::DrawSplineLinesWithColours(int numLines, Vec2f lines[][2], const Spline2& spline, int i, int n, float ts[][2], void* contextIn) 84 | { 85 | DLContext& ctx = *reinterpret_cast(contextIn); 86 | 87 | ColourAlpha c[kMaxEmitLines][2]; 88 | 89 | for (int i = 0; i < numLines; i++) 90 | { 91 | c[i][0] = SplineColour(spline, ts[i][0], ctx.drawMode, ctx.drawModeScale, i); 92 | c[i][1] = SplineColour(spline, ts[i][1], ctx.drawMode, ctx.drawModeScale, i); 93 | } 94 | 95 | ctx.dd->DrawLines(numLines * 2, lines[0], c[0]); 96 | } 97 | 98 | void SL::DrawSplines(Draw* dd, int numSplines, const Spline2 splines[], float tolerance) 99 | { 100 | SplinesToLinesAdaptive(numSplines, splines, ::DrawSplineLines, dd, tolerance); 101 | } 102 | 103 | void SL::DrawSplines(Draw* dd, int numSplines, const Spline2 splines[], SplineDrawMode drawMode, float drawScale, float tolerance) 104 | { 105 | DLContext ctx = { dd, drawMode, drawScale }; 106 | SplinesToLinesAdaptive(numSplines, splines, ::DrawSplineLinesWithColours, &ctx, tolerance); 107 | } 108 | 109 | namespace 110 | { 111 | void OffsetLR(const Spline2& spline, float offset, Spline2* spline_0, Spline2* spline_1) 112 | { 113 | offset *= 0.5f; 114 | 115 | float sx0 = spline.xb.y - spline.xb.x; 116 | float sy0 = spline.yb.y - spline.yb.x; 117 | float sd0 = InvSqrtFast(sx0 * sx0 + sy0 * sy0) * offset; 118 | 119 | float sx1 = spline.xb.z - spline.xb.x; 120 | float sy1 = spline.yb.z - spline.yb.x; 121 | float sd1 = InvSqrtFast(sx1 * sx1 + sy1 * sy1) * offset; 122 | 123 | float sx2 = spline.xb.w - spline.xb.y; 124 | float sy2 = spline.yb.w - spline.yb.y; 125 | float sd2 = InvSqrtFast(sx2 * sx2 + sy2 * sy2) * offset; 126 | 127 | float sx3 = spline.xb.w - spline.xb.z; 128 | float sy3 = spline.yb.w - spline.yb.z; 129 | float sd3 = InvSqrtFast(sx3 * sx3 + sy3 * sy3) * offset; 130 | 131 | Vec4f ox(sy0 * sd0, sy1 * sd1, sy2 * sd2, sy3 * sd3); 132 | Vec4f oy(sx0 * sd0, sx1 * sd1, sx2 * sd2, sx3 * sd3); 133 | 134 | *spline_0 = {spline.xb - ox, spline.yb + oy}; 135 | *spline_1 = {spline.xb + ox, spline.yb - oy}; 136 | } 137 | 138 | void OffsetLR(const Spline3& spline, float offset, Spline3* spline_0, Spline3* spline_1) 139 | { 140 | OffsetLR((const Spline2&) spline, offset, (Spline2*) spline_0, (Spline2*) spline_1); 141 | spline_0->zb = spline.zb; 142 | spline_1->zb = spline.zb; 143 | } 144 | 145 | struct DLContextWidth 146 | { 147 | Draw* dd; 148 | float width; 149 | }; 150 | 151 | void DrawLinesWidth(int numLines, Vec3f lines[][2], const Spline3& spline, int, int, float ts[][2], void* contextIn) 152 | { 153 | DLContextWidth& context = *reinterpret_cast(contextIn); 154 | 155 | Vec3f p[kMaxEmitLines][4]; 156 | 157 | Spline3 os0, os1; 158 | OffsetLR(spline, context.width, &os0, &os1); 159 | 160 | for (int i = 0; i < numLines; i++) 161 | { 162 | p[i][0] = Position(os0, ts[i][0]); 163 | p[i][1] = Position(os0, ts[i][1]); 164 | p[i][2] = Position(os1, ts[i][0]); 165 | p[i][3] = Position(os1, ts[i][1]); 166 | } 167 | 168 | context.dd->DrawLines(4 * numLines, p[0]); 169 | } 170 | 171 | void FillLinesWidth(int numLines, Vec3f lines[][2], const Spline3& spline, int, int, float ts[][2], void* contextIn) 172 | { 173 | DLContextWidth& context = *reinterpret_cast(contextIn); 174 | 175 | Vec3f p[kMaxEmitLines][12]; 176 | 177 | Spline3 os0, os1; 178 | OffsetLR(spline, context.width, &os0, &os1); 179 | 180 | for (int i = 0; i < numLines; i++) 181 | { 182 | Vec3f v00 = Position(os0, ts[i][0]); 183 | Vec3f v01 = Position(os1, ts[i][0]); 184 | Vec3f v10 = Position(os0, ts[i][1]); 185 | Vec3f v11 = Position(os1, ts[i][1]); 186 | 187 | p[i][0] = v00; 188 | p[i][1] = v01; 189 | p[i][2] = v11; 190 | p[i][3] = v00; 191 | p[i][4] = v11; 192 | p[i][5] = v10; 193 | 194 | v00 = cross(norm_safe(lines[i][0]-v00), norm_safe(Velocity(spline, ts[i][0]))); 195 | v10 = cross(norm_safe(lines[i][1]-v10), norm_safe(Velocity(spline, ts[i][1]))); 196 | 197 | v01 = lines[i][0] - v00 * context.width * 0.5f; 198 | v00 = lines[i][0] + v00 * context.width * 0.5f; 199 | v11 = lines[i][1] - v10 * context.width * 0.5f; 200 | v10 = lines[i][1] + v10 * context.width * 0.5f; 201 | 202 | p[i][6] = v00; 203 | p[i][7] = v01; 204 | p[i][8] = v11; 205 | p[i][9] = v00; 206 | p[i][10] = v11; 207 | p[i][11] = v10; 208 | } 209 | 210 | context.dd->DrawTriangles(12 * numLines, p[0]); 211 | } 212 | 213 | void DrawLinesWidth(int numLines, Vec2f lines[][2], const Spline2& spline, int i, int n, float ts[][2], void* contextIn) 214 | { 215 | DLContextWidth& context = *reinterpret_cast(contextIn); 216 | 217 | Vec2f p[kMaxEmitLines][4]; 218 | 219 | Spline2 os0, os1; 220 | OffsetLR(spline, context.width, &os0, &os1); 221 | 222 | for (int i = 0; i < numLines; i++) 223 | { 224 | p[i][0] = Position(os0, ts[i][0]); 225 | p[i][1] = Position(os0, ts[i][1]); 226 | p[i][2] = Position(os1, ts[i][0]); 227 | p[i][3] = Position(os1, ts[i][1]); 228 | } 229 | 230 | context.dd->DrawLines(4 * numLines, p[0]); 231 | } 232 | 233 | void FillLinesWidth(int numLines, Vec2f lines[][2], const Spline2& spline, int i, int n, float ts[][2], void* contextIn) 234 | { 235 | DLContextWidth& context = *reinterpret_cast(contextIn); 236 | 237 | Vec2f p[kMaxEmitLines][6]; 238 | 239 | Spline2 os0, os1; 240 | OffsetLR(spline, context.width, &os0, &os1); 241 | 242 | for (int i = 0; i < numLines; i++) 243 | { 244 | Vec2f v00 = Position(os0, ts[i][0]); 245 | Vec2f v01 = Position(os1, ts[i][0]); 246 | Vec2f v10 = Position(os0, ts[i][1]); 247 | Vec2f v11 = Position(os1, ts[i][1]); 248 | 249 | p[i][0] = v00; 250 | p[i][1] = v01; 251 | p[i][2] = v11; 252 | p[i][3] = v00; 253 | p[i][4] = v11; 254 | p[i][5] = v10; 255 | } 256 | 257 | context.dd->DrawTriangles(6 * numLines, p[0]); 258 | } 259 | } 260 | 261 | 262 | void SL::DrawSplinesWithWidth(Draw* dd, int numSplines, const Spline2 splines[], float width, float tolerance) 263 | { 264 | DLContextWidth context = {dd, width}; 265 | SplinesToLinesAdaptive(numSplines, splines, ::DrawLinesWidth, &context, tolerance); 266 | } 267 | 268 | void SL::FillSplinesWithWidth(Draw* dd, int numSplines, const Spline2 splines[], float width, float tolerance) 269 | { 270 | DLContextWidth context = {dd, width}; 271 | SplinesToLinesAdaptive(numSplines, splines, ::FillLinesWidth, &context, tolerance); 272 | } 273 | 274 | void SL::DrawSplinesWithWidth(Draw* dd, int numSplines, const Spline3 splines[], float width, float tolerance) 275 | { 276 | DLContextWidth context = {dd, width}; 277 | SplinesToLinesAdaptive(numSplines, splines, ::DrawLinesWidth, &context, tolerance); 278 | } 279 | 280 | void SL::FillSplinesWithWidth(Draw* dd, int numSplines, const Spline3 splines[], float width, float tolerance) 281 | { 282 | DLContextWidth context = {dd, width}; 283 | SplinesToLinesAdaptive(numSplines, splines, ::FillLinesWidth, &context, tolerance); 284 | } 285 | 286 | ////////////////////////////////////////////////// 287 | 288 | 289 | void SL::DrawSplinePoints(Draw* dd, int numSplines, const Spline2 splines[], float r) 290 | { 291 | if (numSplines > 0) 292 | { 293 | dd->SetColour(kColourPurple); 294 | 295 | FillCircle(dd, Position0(splines[0 ]), r); 296 | FillCircle(dd, Position1(splines[numSplines - 1]), r); 297 | } 298 | 299 | r *= 0.5f; 300 | dd->SetColour(kColourRed); 301 | 302 | for (int i = 1; i < numSplines; i++) 303 | FillCircle(dd, Position0(splines[i]), r); 304 | } 305 | 306 | void SL::DrawSplineTangents(Draw* dd, int numSplines, const Spline2 splines[], const Colour& colour) 307 | { 308 | dd->SetColour(colour); 309 | 310 | for (int i = 0; i < numSplines; i++) 311 | { 312 | const Spline2& spline = splines[i]; 313 | 314 | Vec2f p0 = Position0(spline); 315 | Vec2f p1 = Position1(spline); 316 | 317 | Vec2f v0 = Velocity0(spline); 318 | Vec2f v1 = Velocity1(spline); 319 | 320 | Vec2f p[] = 321 | { 322 | p0, 323 | p0 + v0 / 3.0f, 324 | p1, 325 | p1 - v1 / 3.0f, 326 | }; 327 | 328 | dd->DrawLines(SL_SIZE(p), p); 329 | } 330 | } 331 | 332 | void SL::DrawSplineFrames(Draw* dd, int numSplines, const Spline2 splines[], float size, float step, bool consistent) 333 | { 334 | Mat2f F = vl_I; 335 | 336 | for (int i = 0; i < numSplines; i++) 337 | { 338 | const Spline2& spline = splines[i]; 339 | 340 | for (float t = 0.5f * step; t <= 1.0f; t += step) 341 | { 342 | Vec2f p = Position(spline, t); 343 | 344 | if (consistent) 345 | Frame(spline, t, &F); 346 | else 347 | F= Frame(spline, t); 348 | 349 | Transform2 xform(size, F, p); 350 | dd->PushTransform2D(xform); 351 | 352 | dd->SetColour(kColourRed); 353 | DrawLine(dd, Vec2f(vl_0), vl_x); 354 | dd->SetColour(kColourGreen); 355 | DrawLine(dd, Vec2f(vl_0), vl_y); 356 | 357 | dd->PopTransform2D(); 358 | } 359 | } 360 | } 361 | 362 | void SL::DrawSplineCurvature(Draw* dd, int numSplines, const Spline2 splines[], float step, float maxR) 363 | { 364 | float minK = 1.0f / maxR; 365 | 366 | for (int i = 0; i < numSplines; i++) 367 | { 368 | const Spline2& spline = splines[i]; 369 | 370 | for (float t = 0.0f; t <= 1.0f; t += step) 371 | { 372 | float k = Curvature(spline, t); 373 | 374 | if (k < minK) 375 | continue; 376 | 377 | Vec2f p = Position(spline, t); 378 | float r = 1.0f / k; 379 | Mat2f F = Frame(spline, t); 380 | 381 | DrawCircle(dd, p + F.y * r, r); 382 | } 383 | } 384 | } 385 | 386 | void SL::DrawSplineExactBounds(Draw* dd, int numSplines, const Spline2 splines[], const Colour& colour) 387 | { 388 | dd->SetColour(colour); 389 | 390 | for (int i = 0; i < numSplines; i++) 391 | DrawRect(dd, ExactBounds(splines[i])); 392 | } 393 | 394 | void SL::DrawSplineFastBounds(Draw* dd, int numSplines, const Spline2 splines[], const Colour& colour) 395 | { 396 | dd->SetColour(colour); 397 | 398 | for (int i = 0; i < numSplines; i++) 399 | DrawRect(dd, FastBounds(splines[i])); 400 | } 401 | 402 | // 3D 403 | 404 | namespace 405 | { 406 | Colour SplineColour(const Spline3& spline, float t, SplineDrawMode mode, float scale, int i) 407 | { 408 | switch (mode) 409 | { 410 | case kSplineDrawModeNone: 411 | scale = 0.0f; 412 | break; 413 | 414 | case kSplineDrawModeT: 415 | scale *= t; 416 | break; 417 | 418 | case kSplineDrawModePosition: 419 | return Fract(scale * Position(spline, t)); 420 | 421 | case kSplineDrawModeVelocity: 422 | scale *= len(Velocity(spline, t)) * 0.001f; 423 | break; 424 | 425 | case kSplineDrawModeAcceleration: 426 | scale *= len(Acceleration(spline, t)) * 0.01f; 427 | 428 | case kSplineDrawModeCurvature: 429 | scale *= Curvature(spline, t) * 100.0f; 430 | break; 431 | 432 | case kSplineDrawModeLength: 433 | scale *= Length(spline, 0.0f, t) * 0.01f; 434 | break; 435 | 436 | case kSplineDrawModeLine: 437 | return LabelColour(i); 438 | break; 439 | 440 | case kNumSplineDrawModes: 441 | SL_ERROR("bad enum"); 442 | } 443 | 444 | scale = ClampUnit(scale); 445 | float hue = lerp(kHueGreen, kHueRed + 360.0f, scale); 446 | 447 | return HSVColour(hue, 1.0f, 1.0f); 448 | } 449 | } 450 | 451 | void SL::DrawSplineLines(int numLines, Vec3f lines[][2], const Spline3&, int, int, float[][2], void* context) 452 | { 453 | reinterpret_cast(context)->DrawLines(2 * numLines, lines[0]); 454 | } 455 | 456 | void SL::DrawSplineLinesWithColours(int numLines, Vec3f lines[][2], const Spline3& spline, int, int, float ts[][2], void* contextIn) 457 | { 458 | DLContext& ctx = *reinterpret_cast(contextIn); 459 | 460 | ColourAlpha c[kMaxEmitLines][2]; 461 | 462 | for (int i = 0; i < numLines; i++) 463 | { 464 | c[i][0] = SplineColour(spline, ts[i][0], ctx.drawMode, ctx.drawModeScale, i); 465 | c[i][1] = SplineColour(spline, ts[i][1], ctx.drawMode, ctx.drawModeScale, i); 466 | } 467 | 468 | ctx.dd->DrawLines(numLines * 2, lines[0], c[0]); 469 | } 470 | 471 | void SL::DrawSplineLines3D(int numLines, Vec2f lines[][2], const Spline2&, int i, int n, float[][2], void* context) 472 | { 473 | Draw* dd = reinterpret_cast(context); 474 | 475 | Vec3f p3[kMaxEmitLines][2]; 476 | 477 | for (int i = 0; i < numLines; i++) 478 | { 479 | p3[i][0] = { lines[i][0].x, lines[i][0].y, 0.01f }; 480 | p3[i][1] = { lines[i][1].x, lines[i][1].y, 0.01f }; 481 | } 482 | 483 | dd->DrawLines(2 * numLines, p3[0]); 484 | } 485 | 486 | void SL::DrawSplineLinesWithColours3D(int numLines, Vec2f lines[][2], const Spline2& spline, int i, int n, float ts[][2], void* contextIn) 487 | { 488 | DLContext& ctx = *reinterpret_cast(contextIn); 489 | 490 | Vec3f p3[kMaxEmitLines][2]; 491 | ColourAlpha c[kMaxEmitLines][2]; 492 | 493 | for (int i = 0; i < numLines; i++) 494 | { 495 | p3[i][0] = { lines[i][0].x, lines[i][0].y, 0.01f }; 496 | p3[i][1] = { lines[i][1].x, lines[i][1].y, 0.01f }; 497 | 498 | c[i][0] = SplineColour(spline, ts[i][0], ctx.drawMode, ctx.drawModeScale, i); 499 | c[i][1] = SplineColour(spline, ts[i][1], ctx.drawMode, ctx.drawModeScale, i); 500 | } 501 | 502 | ctx.dd->DrawLines(numLines * 2, p3[0], c[0]); 503 | } 504 | 505 | void SL::DrawSplines(Draw* dd, int numSplines, const Spline3 splines[], float tolerance) 506 | { 507 | SplinesToLinesAdaptive(numSplines, splines, ::DrawSplineLines, dd, tolerance); 508 | } 509 | 510 | void SL::DrawSplines(Draw* dd, int numSplines, const Spline3 splines[], SplineDrawMode drawMode, float drawScale, float tolerance) 511 | { 512 | DLContext ctx = { dd, SplineDrawMode(drawMode), drawScale }; 513 | SplinesToLinesAdaptive(numSplines, splines, ::DrawSplineLinesWithColours, &ctx, tolerance); 514 | } 515 | 516 | void SL::DrawSplinePoints(Draw* dd, int numSplines, const Spline3 splines[], float r, Vec3f normal) 517 | { 518 | if (numSplines > 0) 519 | { 520 | dd->SetColour(kColourPurple); 521 | 522 | FillCircle(dd, Position0(splines[0 ]), r, normal); 523 | FillCircle(dd, Position1(splines[numSplines - 1]), r, normal); 524 | } 525 | 526 | r *= 0.5f; 527 | dd->SetColour(kColourRed); 528 | 529 | for (int i = 1; i < numSplines; i++) 530 | FillCircle(dd, Position0(splines[i]), r, normal); 531 | } 532 | 533 | void SL::DrawSplineTangents(Draw* dd, int numSplines, const Spline3 splines[], const Colour& colour) 534 | { 535 | dd->SetColour(colour); 536 | 537 | for (int i = 0; i < numSplines; i++) 538 | { 539 | const Spline3& spline = splines[i]; 540 | 541 | Vec3f p0 = Position0(spline); 542 | Vec3f p1 = Position1(spline); 543 | 544 | Vec3f v0 = Velocity0(spline); 545 | Vec3f v1 = Velocity1(spline); 546 | 547 | Vec3f p[] = 548 | { 549 | p0, 550 | p0 + v0 / 3.0f, 551 | p1, 552 | p1 - v1 / 3.0f, 553 | }; 554 | 555 | dd->DrawLines(SL_SIZE(p), p); 556 | } 557 | } 558 | 559 | void SL::DrawSplineFrames(Draw* dd, int numSplines, const Spline3 splines[], float size, float step, bool consistent) 560 | { 561 | Mat3f F = vl_I; 562 | 563 | for (int i = 0; i < numSplines; i++) 564 | { 565 | const Spline3& spline = splines[i]; 566 | 567 | for (float t = 0.5f * step; t <= 1.0f; t += step) 568 | { 569 | Vec3f p = Position(spline, t); 570 | 571 | if (consistent) 572 | Frame(spline, t, &F); 573 | else 574 | F = Frame(spline, t); 575 | 576 | Transform3 xform(size, F, p); 577 | dd->PushTransform3D(xform); 578 | 579 | dd->SetColour(kColourRed); 580 | DrawLine(dd, Vec3f(vl_0), vl_x); 581 | dd->SetColour(kColourGreen); 582 | DrawLine(dd, Vec3f(vl_0), vl_y); 583 | dd->SetColour(kColourBlue); 584 | DrawLine(dd, Vec3f(vl_0), vl_z); 585 | 586 | dd->PopTransform3D(); 587 | } 588 | } 589 | } 590 | 591 | void SL::DrawSplineCurvature(Draw* dd, int numSplines, const Spline3 splines[], float step, float maxR) 592 | { 593 | float minK = 1.0f / maxR; 594 | 595 | for (int i = 0; i < numSplines; i++) 596 | { 597 | const Spline3& spline = splines[i]; 598 | 599 | for (float t = 0.5f * step; t <= 1.0f; t += step) 600 | { 601 | float k = Curvature(spline, t); 602 | 603 | if (k < minK) 604 | continue; 605 | 606 | Vec3f p = Position(spline, t); 607 | float r = 1.0f / k; 608 | Mat3f F = Frame(spline, t); 609 | 610 | Transform3 xform(r, F, p); 611 | dd->PushTransform3D(xform); 612 | DrawCircle(dd, Vec3f(vl_y), 1.0f, Vec3f(vl_z)); 613 | dd->PopTransform3D(); 614 | } 615 | } 616 | } 617 | 618 | void SL::DrawSplineTorsion(Draw* dd, int numSplines, const Spline3 splines[], float step, float scale) 619 | { 620 | Vec3f c0 = dd->Colour(); 621 | Vec3f c1 = Vec3f(vl_1) - c0; 622 | 623 | for (int i = 0; i < numSplines; i++) 624 | { 625 | const Spline3& spline = splines[i]; 626 | 627 | for (float t = 0.5f * step; t <= 1.0f; t += step) 628 | { 629 | float tt = Torsion(spline, t); 630 | float r = fabsf(tt * scale); 631 | 632 | if (r < 1e-1f) 633 | continue; 634 | 635 | Vec3f p = Position(spline, t); 636 | Mat3f F = Frame(spline, t); 637 | 638 | Transform3 xform(r, F, p); 639 | dd->PushTransform3D(xform); 640 | if (tt < 0.0f) 641 | dd->SetColour(c1); 642 | else 643 | dd->SetColour(c0); 644 | DrawCircle(dd, Vec3f(vl_0), 1.0f, Vec3f(vl_x)); 645 | dd->PopTransform3D(); 646 | } 647 | } 648 | 649 | dd->SetColour(c0); 650 | } 651 | 652 | void SL::DrawSplineExactBounds(Draw* dd, int numSplines, const Spline3 splines[], const Colour& colour) 653 | { 654 | dd->SetColour(colour); 655 | 656 | for (int i = 0; i < numSplines; i++) 657 | { 658 | const Spline3& spline = splines[i]; 659 | 660 | Bounds3 bounds = ExactBounds(spline); 661 | 662 | DrawBox(dd, bounds.mMin, bounds.mMax); 663 | } 664 | } 665 | 666 | void SL::DrawSplineFastBounds(Draw* dd, int numSplines, const Spline3 splines[], const Colour& colour) 667 | { 668 | dd->SetColour(colour); 669 | 670 | for (int i = 0; i < numSplines; i++) 671 | { 672 | const Spline3& spline = splines[i]; 673 | 674 | Bounds3 bounds = FastBounds(spline); 675 | 676 | DrawBox(dd, bounds.mMin, bounds.mMax); 677 | } 678 | } 679 | -------------------------------------------------------------------------------- /Splines.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Splines.hpp 3 | // 4 | // Cubic spline utilities for 1D/2D/3D 5 | // 6 | // Andrew Willmott 7 | // 8 | 9 | #ifndef SL_SPLINES_H 10 | #define SL_SPLINES_H 11 | 12 | #include "VLMini.hpp" 13 | #include 14 | 15 | #ifndef SL_ASSERT 16 | #define SL_ASSERT(X) 17 | #endif 18 | 19 | namespace SL 20 | { 21 | typedef Vec2f Bounds1; 22 | struct Bounds2 { Vec2f mMin; Vec2f mMax; }; 23 | struct Bounds3 { Vec3f mMin; Vec3f mMax; }; 24 | 25 | // 1D 26 | typedef Vec4f Spline1; 27 | 28 | // Spline creation 29 | Spline1 BezierSpline (float p0, float p1, float p2, float p3); // Returns Bezier spline from p0 to p3 with guide points p1, p2 30 | Spline1 HermiteSpline (float p0, float p1, float v0, float v1); // Returns Hermite spline from p0 to p1 with corresponding tangents (slopes) v0, v1. 31 | Spline1 CatmullRomSpline (float p0, float p1, float p2, float p3); // Returns Catmull-Rom spline passing through p1 and p2, with tangents affected by p0 and p3. 32 | Spline1 InterpolatingSpline(float p0, float p1, float p2, float p3); // Returns spline that interpolates p0, p1, p2, p3 at t=0,1/3,2/3,1 33 | Spline1 LineSpline (float p0, float p1); // Returns a spline representing the line p0_p1 34 | Spline1 CubicSpline (const Vec4f& abcd); // Returns a spline representing a + bx + cx^2 + dx^3 35 | 36 | int NumSplinesForPoints(int numPoints); // Returns number of splines needed to represent the given number of points; generally n-1 except for n < 2. 37 | int SplinesFromPoints (int numPoints, const float p[], Spline1 splines[], float tension = 0.0f, size_t stride = sizeof(float)); 38 | // Fills 'splines' with splines that interpolate the values in 'p', and returns the number of these splines. 39 | // 'tension' controls the interpolation -- the default value of 0 specifies Catmull-Rom splines that guarantee tangent continuity. With +1 you get linear interpolation, and -1 gives exaggerated smoothing. 40 | 41 | int SplinesFromPointsDynamic(int numPoints, const float p[], Spline1 splines[], float tension = 0.0f, float overshootRatio = 0.5f, size_t stride = sizeof(Vec2f)); 42 | // A version of SplinesFromPoints that adjusts tension per point to avoid overshoots/loops 43 | // caused by disparate segment sizes. 44 | 45 | int SplinesFromBezier (int numPoints, const float points[], const float hullPoints[], Spline1 splines[], bool split = false); // Creates splines from the given points and Bezier hull points. If 'split' is false the splines are assumed to be continuous and numPoints - 1 splines are output. Otherwise the points are assumed to come in pairs, and numPoints / 2 splines output. 46 | int SplinesFromHermite(int numPoints, const float points[], const float tangents [], Spline1 splines[], bool split = false); // Creates splines from the given points and tangents. If 'split' is false the splines are assumed to be continuous and numPoints - 1 splines are output. Otherwise the points are assumed to come in pairs, and numPoints / 2 splines output. 47 | 48 | void MakeMonotonic(int numSplines, Spline1 splines[], bool closed = false); 49 | // Makes splines monotonic (internal spline values always within the end points) by adjusting tangents. Useful for situations where overshoots lead to invalid values. 50 | 51 | // Queries 52 | float Position0(const Spline1& spline); // Start of spline 53 | float Position1(const Spline1& spline); // End of spline 54 | float Velocity0(const Spline1& spline); // Start velocity (dx/dt) 55 | float Velocity1(const Spline1& spline); // End velocity 56 | 57 | float Position (const Spline1& spline, float t); // Returns position at 't' (x) 58 | float Velocity (const Spline1& spline, float t); // Returns velocity at 't' (x') 59 | float Acceleration(const Spline1& spline, float t); // Returns acceleration at 't' (x'') 60 | float Jerk (const Spline1& spline, float t); // Returns jerk at 't' (x''') -- for cubic splines this is constant 61 | 62 | Bounds1 FastBounds (const Spline1& spline); // Returns fast, convervative bounds based off the convex hull of the spline. 63 | Bounds1 ExactBounds(const Spline1& spline); // Returns exact bounds, taking into account extrema, requires solving a quadratic 64 | 65 | // Conversion 66 | Vec4f CubicCoeffs(const Spline1& spline); // Returns the spline as cubic coefficients, [a, b, c, d] 67 | 68 | // Subdivision 69 | void Split(const Spline1& spline, Spline1* spline0, Spline1* spline1); // Splits 'spline' into two halves (at t = 0.5) and stores the results in 'subSplines' 70 | void Split(const Spline1& spline, float t, Spline1* spline0, Spline1* spline1); // Fills 'subSplines' with the splines corresponding to [0, t] and [t, 1] 71 | bool Join (const Spline1& spline0, const Spline1& spline1, Spline1* spline); // Joins two splines that were formerly Split(). Assumes t=0.5, returns false if the source splines don't match up. 72 | 73 | Spline1 Trim(const Spline1& spline, float t0, float t1); // Returns subsection of 'spline' between t0 and t1 74 | 75 | 76 | // 2D 77 | struct Spline2 78 | { 79 | Spline1 xb; // x cubic bezier coefficients 80 | Spline1 yb; // y cubic bezier coefficients 81 | }; 82 | 83 | // Spline creation 84 | Spline2 BezierSpline (Vec2f p0, Vec2f p1, Vec2f p2, Vec2f p3); // Returns Bezier spline from p0 to p3 with guide points p1, p2 85 | Spline2 HermiteSpline (Vec2f p0, Vec2f p1, Vec2f v0, Vec2f v1); // Returns Hermite spline from p0 to p1 with corresponding tangents v0, v1. 86 | Spline2 CatmullRomSpline (Vec2f p0, Vec2f p1, Vec2f p2, Vec2f p3); // Returns Catmull-Rom spline passing through p1 and p2, with tangents affected by p0 and p3. 87 | Spline2 InterpolatingSpline(Vec2f p0, Vec2f p1, Vec2f p2, Vec2f p3); // Returns spline that interpolates p0, p1, p2, p3 88 | 89 | Spline2 LineSpline (Vec2f p0, Vec2f p1); // Returns a spline representing the line p0_p1 90 | Spline2 QuadrantSpline (Vec2f p, float r, int quadrant); // Returns a spline representing the given quadrant (quarter circle) of radius 'r' at 'p' 91 | void CircleSplines (Vec2f p, float r, Spline2 splines[4]); // Fills 'splines' with four splines representing a circle of radius 'r' at 'p' 92 | 93 | int NumSplinesForPoints(int numPoints); // Returns number of splines needed to represent the given number of points; generally n-1 except for n < 2. 94 | int SplinesFromPoints (int numPoints, const Vec2f p[], Spline2 splines[], float tension = 0.0f, size_t stride = sizeof(Vec2f)); 95 | // Fills 'splines' with splines that interpolate the points in 'p', and returns the number of these splines. 96 | // 'tension' controls the interpolation -- the default value of 0 specifies Catmull-Rom splines that 97 | // guarantee tangent continuity. With +1 you get straight lines, and -1 gives more of a circular appearance. 98 | 99 | int SplinesFromPointsDynamic(int numPoints, const Vec2f p[], Spline2 splines[], float tension = 0.0f, float overshootRatio = 0.5f, size_t stride = sizeof(Vec2f)); 100 | // A version of SplinesFromPoints that adjusts tension per point to avoid overshoots/loops 101 | // caused by disparate segment sizes. 102 | 103 | int SplinesFromBezier (int numPoints, const Vec2f points[], const Vec2f hullPoints[], Spline2 splines[], bool split = false); // Creates splines from the given points and Bezier hull points. If 'split' is false the splines are assumed to be continuous and numPoints - 1 splines are output. Otherwise the points are assumed to come in pairs, and numPoints / 2 splines output. 104 | int SplinesFromHermite(int numPoints, const Vec2f points[], const Vec2f tangents [], Spline2 splines[], bool split = false); // Creates splines from the given points and tangents. If 'split' is false the splines are assumed to be continuous and numPoints - 1 splines are output. Otherwise the points are assumed to come in pairs, and numPoints / 2 splines output. 105 | 106 | void MakeMonotonic(int numSplines, Spline2 splines[], bool closed = false); 107 | // Makes splines monotonic (internal spline values always within the end points) by adjusting tangents. Useful for situations where overshoots lead to invalid values. 108 | 109 | // Queries 110 | Vec2f Position0(const Spline2& spline); // Start of spline 111 | Vec2f Position1(const Spline2& spline); // End of spline 112 | Vec2f Velocity0(const Spline2& spline); // Start velocity (tangent x dp/dt) 113 | Vec2f Velocity1(const Spline2& spline); // End velocity 114 | 115 | Vec2f Position (const Spline2& spline, float t); // Returns position at 't' 116 | Vec2f Velocity (const Spline2& spline, float t); // Returns velocity at 't' 117 | Vec2f Acceleration(const Spline2& spline, float t); // Returns acceleration at 't' 118 | Vec2f Jerk (const Spline2& spline, float t); // Returns jerk at 't' -- for cubic splines this is constant 119 | 120 | float Curvature (const Spline2& spline, float t); // Returns curvature at 't'. Curvature = 1 / r, where r is the radius of the osculating circle, so 0 on a straight path. 121 | 122 | Mat2f Frame (const Spline2& spline, float t); // Returns the Frenet frame at 't': x=forward, y=left/right, depending on which way the path is curving. 123 | void Frame (const Spline2& spline, float t, Mat2f* frame); // Updates 'frame' using the Frenet frame at 't', maintaining consistency by avoiding any axis flips. 124 | Mat2f FrameX (const Spline2& spline, float t); // Returns frame with x forwards and y=left 125 | 126 | float LengthEstimate(const Spline2& s, float* error); // Returns estimate of length of s and optionally in 'error' the maximum error of that length. 127 | float Length (const Spline2& s, float maxError = 0.01f); // Returns length of spline accurate to the given tolerance, using multiple LengthEstimate() calls. 128 | float Length (const Spline2& s, float t0, float t1, float maxError = 0.01f); // Returns length of spline segment over [t0, t1]. 129 | 130 | Bounds2 FastBounds (const Spline2& spline); // Returns fast, convervative bounds based off the convex hull of the spline. 131 | Bounds2 ExactBounds(const Spline2& spline); // Returns exact bounds, taking into account extrema, requires solving a quadratic 132 | 133 | // Line conversion 134 | constexpr int kMaxEmitLines = 128; 135 | typedef void tEmitLinesFunc2(int numLines, Vec2f lines[][2], void* context); // Called with up to kMaxEmitLines lines at a time 136 | int SplinesToLinesLinear (int numSplines, const Spline2 splines[], tEmitLinesFunc2 emitLines, void* context, float step = 0.05f); // Convert given splines into lines using even steps in 't' 137 | int SplinesToLinesAdaptive(int numSplines, const Spline2 splines[], tEmitLinesFunc2 emitLines, void* context, float tolerance = 0.05f); // Convert given splines into lines adaptively, with given tolerance in world units. 138 | 139 | typedef void tEmitLinesExFunc2(int numLines, Vec2f lines[][2], const Spline2& spline, int i, int n, float ts[][2], void* context); // Called with up to kMaxEmitLines lines at a time, plus t values and owning spline. 140 | int SplinesToLinesLinear (int numSplines, const Spline2 splines[], tEmitLinesExFunc2 emitLines, void* context, float step = 0.05f); 141 | int SplinesToLinesAdaptive(int numSplines, const Spline2 splines[], tEmitLinesExFunc2 emitLines, void* context, float tolerance = 0.05f); 142 | 143 | // Subdivision 144 | void Split(const Spline2& spline, Spline2* spline0, Spline2* spline1); // Splits 'spline' into two halves (at t = 0.5) and stores the results in 'subSplines' 145 | void Split(const Spline2& spline, float t, Spline2* spline0, Spline2* spline1); // Fills 'subSplines' with the splines corresponding to [0, t] and [t, 1] 146 | bool Join (const Spline2& spline0, const Spline2& spline1, Spline2* spline); // Joins two splines that were formerly Split(). Assumes t=0.5, returns false if the source splines don't match up. 147 | 148 | void Split(std::vector* splines); // Subdivide each spline into two pieces 149 | void Split(std::vector* splines, int n); // Subdivide each spline into 'n' pieces 150 | void Join (std::vector* splines); // Join adjacent splines where possible 151 | 152 | Spline2 Trim(const Spline2& spline, float t0, float t1); // Returns subsection of 'spline' between t0 and t1 153 | 154 | void SubdivideForLength(std::vector* splines, float relativeError = 0.01f); // Subdivide splines to be close to linear, according to relativeError. 155 | void SubdivideForT (std::vector* splines, float error = 0.01f); // Subdivide splines to be close to linear in t, i.e., arcLength 156 | 157 | void SubdivideForLengthRatio(std::vector& positions, float maxRatio = 8.0f); // Insert additional positions as necessary to ensure no segment is more than 'maxRatio' times its neighbours. Useful as preconditioner for SplinesFromPoints. 158 | 159 | // Nearest point 160 | float FindClosestPoint(Vec2f p, const Spline2& spline); // Returns t value of the closest point on s to 'p' 161 | float FindClosestPoint(Vec2f p, int numSplines, const Spline2 splines[], int* index); // Returns index of nearest spline, and 't' value of nearest point on that spline. 162 | 163 | struct SubSpline2 164 | { 165 | Spline2 mSpline; 166 | int mParent; 167 | float mD2; 168 | }; 169 | 170 | int FindNearbySplines(Vec2f p, int numSplines, const Spline2 splines[], std::vector* nearbySplines, float* smallestFarOut = 0, int maxIter = 2); 171 | float FindClosestPoint (Vec2f p, int numSplines, const Spline2 splines[], const std::vector& nearbySplines, int* index); 172 | 173 | int FindSplineIntersections(const Spline2& spline0, const Spline2& spline1, int maxResults, float results[][2], float tolerance = 0.1f); 174 | // Returns up to 'maxResults' intersections between the two splines, accurate to the given tolerance. 175 | int FindSplineIntersections(int numSplines0, const Spline2 splines0[], int numSplines1, const Spline2 splines1[], int maxResults, int resultsI[][2], float resultsT[][2], float tolerance = 0.1f); 176 | // Returns up to 'maxResults' intersections between the two spline lists, accurate to the given tolerance. 177 | int FindSplineIntersections(int numSplines, const Spline2 splines[], int maxResults, int resultsI[][2], float resultsT[][2], float tolerance = 0.1f); 178 | // Returns up to 'maxResults' self-intersections in the given spline list, accurate to the given tolerance. 179 | 180 | // Linear point movement along spline set 181 | float AdvanceAgent(const Spline2& spline, float t, float dl); // Advances 'agent' at 't' on the given spline, by dl (delta length), returning t' of the new location. 182 | 183 | bool AdvanceAgent(int* index, float* t, int numSplines, const Spline2 splines[], float dl); // Version of AdvanceAgent for a set of splines, assumed to be continuous. Returns false if the agent has gone off the end, in which case call ClampAgent/WrapAgent/ReverseAgent. 184 | void ClampAgent (int* index, float* t, int numSplines); // Clamps agent position back to the nearest endpoint. 185 | void WrapAgent (int* index, float* t, int numSplines); // Wraps the agent from the end back to the start, or vice versa. 186 | void ReverseAgent(int* index, float* t); // Reverses the agent. (You must also negate the sign of dl yourself.) 187 | 188 | // Misc operations 189 | Spline2 Reverse(const Spline2& spline); // Reverses spline endpoints and tangents so that g(t) = f(1 - t). 190 | Spline2 Offset (const Spline2& spline, float offset); // Offset spline, e.g., for stroking, +ve = to the right. 191 | 192 | void Reverse(std::vector* splines); // Reverses entire spline list 193 | void Offset (std::vector* splines, float offset); // Offset splines, e.g., for stroking, +ve = to the right. 194 | 195 | 196 | // 3D 197 | struct Spline3 198 | { 199 | Spline1 xb; // x cubic bezier coefficients 200 | Spline1 yb; // y cubic bezier coefficients 201 | Spline1 zb; // z cubic bezier coefficients 202 | }; 203 | 204 | // Spline creation 205 | Spline3 BezierSpline (Vec3f p0, Vec3f p1, Vec3f p2, Vec3f p3); // Returns Bezier spline from p0 to p3 with guide points p1, p2 206 | Spline3 HermiteSpline (Vec3f p0, Vec3f p1, Vec3f v0, Vec3f v1); // Returns Hermite spline from p0 to p1 with corresponding tangents v0, v1. 207 | Spline3 CatmullRomSpline (Vec3f p0, Vec3f p1, Vec3f p2, Vec3f p3); // Returns Catmull-Rom spline passing through p1 and p2, with tangents affected by p0 and p3. 208 | Spline3 InterpolatingSpline(Vec3f p0, Vec3f p1, Vec3f p2, Vec3f p3); // Returns spline that interpolates p0, p1, p2, p3. 209 | 210 | Spline3 LineSpline (Vec3f p0, Vec3f p1); // Returns a spline representing the line p0_p1 211 | Spline3 QuadrantSpline (Vec3f p, float r, int quadrant); // Returns a spline representing the given quadrant (quarter circle) of radius 'r' at 'p' 212 | void CircleSplines (Vec3f p, float r, Spline3 splines[4]); // Fills 'splines' with four splines representing a circle of radius 'r' at 'p' 213 | 214 | int NumSplinesForPoints(int numPoints); // Returns number of splines needed to represent the given number of points; generally n-1 except for n < 2. 215 | int SplinesFromPoints (int numPoints, const Vec3f p[], Spline3 splines[], float tension = 0.0f, size_t stride = sizeof(Vec3f)); 216 | // Fills 'splines' with splines that interpolate the points in 'p', and returns the number of these splines. 217 | // 'tension' controls the interpolation -- the default value of 0 specifies Catmull-Rom splines that 218 | // guarantee tangent continuity. With +1 you get straight lines, and -1 gives more of a circular appearance. 219 | 220 | int SplinesFromPointsDynamic(int numPoints, const Vec3f p[], Spline3 splines[], float tension = 0.0f, float overshootRatio = 0.5f, size_t stride = sizeof(Vec3f)); 221 | // A version of SplinesFromPoints that adjusts tension per point to avoid overshoots/loops 222 | // caused by disparate segment sizes. 223 | 224 | int SplinesFromBezier (int numPoints, const Vec3f points[], const Vec3f hullPoints[], Spline3 splines[], bool split = false); // Creates splines from the given points and Bezier hull points. If 'split' is false the splines are assumed to be continuous and numPoints - 1 splines are output. Otherwise the points are assumed to come in pairs, and numPoints / 2 splines output. 225 | int SplinesFromHermite(int numPoints, const Vec3f points[], const Vec3f tangents [], Spline3 splines[], bool split = false); // Creates splines from the given points and tangents. If 'split' is false the splines are assumed to be continuous and numPoints - 1 splines are output. Otherwise the points are assumed to come in pairs, and numPoints / 2 splines output. 226 | 227 | void MakeMonotonic(int numSplines, Spline3 splines[], bool closed = false); // closed = splines are a closed path 228 | // Makes splines monotonic (internal spline values always within the end points) by adjusting tangents. Useful for situations where overshoots lead to invalid values, e.g., RGB colours. 229 | 230 | // Queries 231 | Vec3f Position0(const Spline3& spline); // Start of spline 232 | Vec3f Position1(const Spline3& spline); // End of spline 233 | 234 | Vec3f Velocity0(const Spline3& spline); // Start velocity (tangent x dp/dt) 235 | Vec3f Velocity1(const Spline3& spline); // End velocity 236 | 237 | Vec3f Position (const Spline3& spline, float t); // Returns position at 't' 238 | Vec3f Velocity (const Spline3& spline, float t); // Returns velocity at 't' 239 | Vec3f Acceleration(const Spline3& spline, float t); // Returns acceleration at 't' 240 | Vec3f Jerk (const Spline3& spline, float t); // Returns jerk at 't' -- for cubic splines this is constant over the spline 241 | 242 | float Curvature (const Spline3& spline, float t); // Returns curvature at 't'. Curvature = 1 / r, where r is the radius of the osculating circle, so 0 on a straight path. 243 | float Torsion (const Spline3& spline, float t); // Returns torsion at 't'. This is a measure of how the plane of curvature rotates along the path, so 0 for a flat path. Unlike curvature it is signed. 244 | 245 | Mat3f Frame (const Spline3& spline, float t); // Returns the classic Frenet frame at 't': tangent/normal/binormal. x=forward, y=left/right depending on direction of curvature, z=up/down depending on y. 246 | void Frame (const Spline3& spline, float t, Mat3f* frame); // Updates 'frame' using the Frenet frame at 't', maintaining consistency by avoiding any axis flips. 247 | Mat3f FrameX (const Spline3& spline, float t); // Returns frame with x forwards and y=left, z=up 248 | Mat3f FrameX (const Spline3& spline, float t, Vec3f up); // Returns right-handed frame with x forwards and the given up vector. 249 | 250 | float LengthEstimate(const Spline3& s, float* error); // Returns estimate of length of s and optionally in 'error' the maximum error of that length. 251 | float Length (const Spline3& s, float maxError = 0.01f); // Returns length of spline accurate to the given tolerance 252 | float Length (const Spline3& s, float t0, float t1, float maxError = 0.01f); // Returns length of spline segment over [t0, t1]. 253 | 254 | Bounds3 FastBounds (const Spline3& spline); // Returns fast, convervative bounds based off the convex hull of the spline. 255 | Bounds3 ExactBounds(const Spline3& spline); // Returns exact bounds, taking into account extrema, requires solving a quadratic 256 | 257 | // Line conversion 258 | typedef void tEmitLinesFunc3(int numLines, Vec3f lines[][2], void* context); // Called with up to kMaxEmitLines lines at a time 259 | int SplinesToLinesLinear (int numSplines, const Spline3 splines[], tEmitLinesFunc3 emitLines, void* context, float step = 0.05f); // Convert given splines into lines using even steps in 't' 260 | int SplinesToLinesAdaptive(int numSplines, const Spline3 splines[], tEmitLinesFunc3 emitLines, void* context, float tolerance = 0.05f); // Convert given splines into lines adaptively, with given tolerance in world units. 261 | 262 | typedef void tEmitLinesExFunc3(int numLines, Vec3f lines[][2], const Spline3& spline, int i, int n, float ts[][2], void* context); // Called with up to kMaxEmitLines lines at a time, plus t values and owning spline. 263 | int SplinesToLinesLinear (int numSplines, const Spline3 splines[], tEmitLinesExFunc3 emitLines, void* context, float step = 0.05f); 264 | int SplinesToLinesAdaptive(int numSplines, const Spline3 splines[], tEmitLinesExFunc3 emitLines, void* context, float tolerance = 0.05f); 265 | 266 | // Subdivision 267 | void Split(const Spline3& spline, Spline3* spline0, Spline3* spline1); // Splits 'spline' into two halves (at t = 0.5) and stores the results in 'subSplines' 268 | void Split(const Spline3& spline, float t, Spline3* spline0, Spline3* spline1); // Fills 'subSplines' with the splines corresponding to [0, t] and [t, 1] 269 | bool Join (const Spline3& spline0, const Spline3& spline1, Spline3* spline); // Joins two splines that were formerly Split(). Assumes t=0.5, returns false if the source splines don't match up. 270 | 271 | void Split(std::vector* splines); // Subdivide each spline into two pieces 272 | void Split(std::vector* splines, int n); // Subdivide each spline into 'n' pieces 273 | void Join (std::vector* splines); // Join adjacent splines where possible 274 | 275 | Spline3 Trim(const Spline3& spline, float t0, float t1); // Returns subsection of 'spline' between t0 and t1 276 | 277 | void SubdivideForLength(std::vector* splines, float relativeError = 0.01f); // Subdivide splines to be close to linear, according to relativeError. 278 | void SubdivideForT (std::vector* splines, float error = 0.01f); // Subdivide splines to be close to linear in t, i.e., arcLength 279 | 280 | void SubdivideForLengthRatio(std::vector& positions, float maxRatio = 8.0f); // Insert additional positions as necessary to ensure no segment is more than 'maxRatio' times its neighbours. Useful as preconditioner for SplinesFromPoints. 281 | 282 | // Nearest point 283 | float FindClosestPoint(Vec3f p, const Spline3& spline); // Returns t value of the closest point on s to 'p' 284 | float FindClosestPoint(Vec3f p, int numSplines, const Spline3 splines[], int* index); // Returns index of nearest spline, and 't' value of nearest point on that spline. 285 | 286 | struct SubSpline3 287 | { 288 | Spline3 mSpline; 289 | int mParent; 290 | float mD2; 291 | }; 292 | 293 | int FindNearbySplines(Vec3f p, int numSplines, const Spline3 splines[], std::vector* nearbySplines, float* smallestFarOut = 0, int maxIter = 2); 294 | float FindClosestPoint (Vec3f p, int numSplines, const Spline3 splines[], const std::vector& nearbySplines, int* index); 295 | 296 | int FindSplineIntersections(const Spline3& spline0, const Spline3& spline1, int maxResults, float results[][2], float tolerance = 0.1f); 297 | // Returns up to 'maxResults' intersections between the two splines, accurate to the given tolerance. 298 | int FindSplineIntersections(int numSplines0, const Spline3 splines0[], int numSplines1, const Spline3 splines1[], int maxResults, int resultsI[][2], float resultsT[][2], float tolerance = 0.1f); 299 | // Returns up to 'maxResults' intersections between the two spline lists, accurate to the given tolerance. 300 | int FindSplineIntersections(int numSplines, const Spline3 splines[], int maxResults, int resultsI[][2], float resultsT[][2], float tolerance = 0.1f); 301 | // Returns up to 'maxResults' self-intersections in the given spline list, accurate to the given tolerance. 302 | 303 | // Linear point movement along spline set 304 | float AdvanceAgent(const Spline3& spline, float t, float dl); // Advances 'agent' at 't' on the given spline, by dl (delta length), returning t' of the new location. 305 | 306 | bool AdvanceAgent(int* index, float* t, int numSplines, const Spline3 splines[], float dl); // Version of AdvanceAgent for a set of splines, assumed to be continuous. Returns false if the agent has gone off the end, in which case call ClampAgent/WrapAgent/ReverseAgent. 307 | void ClampAgent (int* index, float* t, int numSplines); // Clamps agent position back to the nearest endpoint. 308 | void WrapAgent (int* index, float* t, int numSplines); // Wraps the agent from the end back to the start, or vice versa. 309 | void ReverseAgent(int* index, float* t); // Reverses the agent. (You must also negate the sign of dl yourself.) 310 | 311 | // Misc operations 312 | Spline3 Reverse(const Spline3& spline); // Reverses spline endpoints and tangents so that g(t) = f(1 - t). 313 | Spline3 Offset (const Spline3& spline, float offset); // Offset spline, e.g., for stroking, +ve = to the right. 314 | 315 | void Reverse(std::vector* splines); // Reverses entire spline list 316 | void Offset (std::vector* splines, float offset); // Offset splines, e.g., for stroking, +ve = to the right. 317 | } 318 | 319 | 320 | // --- Inlines ----------------------------------------------------------------- 321 | 322 | inline int SL::NumSplinesForPoints(int numPoints) 323 | { 324 | if (numPoints < 2) 325 | return numPoints; 326 | 327 | return numPoints - 1; 328 | } 329 | 330 | namespace 331 | { 332 | inline Vec4f BezierWeights(float t) 333 | // Returns Bezier basis weights for 't' 334 | { 335 | float s = 1.0f - t; 336 | 337 | float t2 = t * t; 338 | float t3 = t2 * t; 339 | 340 | float s2 = s * s; 341 | float s3 = s2 * s; 342 | 343 | return Vec4f(s3, 3.0f * s2 * t, 3.0f * s * t2, t3); 344 | } 345 | 346 | inline Vec4f BezierWeightsD1(float t) 347 | { 348 | float t2 = t * t; 349 | 350 | return Vec4f 351 | ( 352 | - 3.0f + 6.0f * t - 3.0f * t2, 353 | 3.0f - 12.0f * t + 9.0f * t2, 354 | 6.0f * t - 9.0f * t2, 355 | 3.0f * t2 356 | ); 357 | } 358 | 359 | inline Vec4f BezierWeightsD2(float t) 360 | { 361 | return Vec4f 362 | ( 363 | 6.0f - 6.0f * t, 364 | -12.0f + 18.0f * t, 365 | 6.0f - 18.0f * t, 366 | 6.0f * t 367 | ); 368 | } 369 | 370 | const Vec4f kBezierWeightsD3(-6.0f, 18.0f, -18.0f, 6.0f); 371 | } 372 | 373 | 374 | // 1D 375 | 376 | inline float SL::Position0(const Spline1& spline) 377 | { 378 | return spline.x; 379 | } 380 | inline float SL::Position1(const Spline1& spline) 381 | { 382 | return spline.w; 383 | } 384 | 385 | inline float SL::Velocity0(const Spline1& spline) 386 | { 387 | return 3.0f * (spline.y - spline.x); 388 | } 389 | inline float SL::Velocity1(const Spline1& spline) 390 | { 391 | return 3.0f * (spline.w - spline.z); 392 | } 393 | 394 | inline float SL::Position(const Spline1& spline, float t) 395 | { 396 | return dot(spline, BezierWeights(t)); 397 | } 398 | 399 | inline float SL::Velocity(const Spline1& spline, float t) 400 | { 401 | return dot(spline, BezierWeightsD1(t)); 402 | } 403 | 404 | inline float SL::Acceleration(const Spline1& spline, float t) 405 | { 406 | return dot(spline, BezierWeightsD2(t)); 407 | } 408 | 409 | inline float SL::Jerk(const Spline1& spline, float) 410 | { 411 | return dot(spline, kBezierWeightsD3); 412 | } 413 | 414 | inline Vec4f SL::CubicCoeffs(const Spline1& b) 415 | { 416 | return Vec4f 417 | ( 418 | b.x , 419 | -3.0f * b.x + 3.0f * b.y , 420 | 3.0f * b.x - 6.0f * b.y + 3.0f * b.z , 421 | -b.x + 3.0f * b.y - 3.0f * b.z + b.w 422 | ); 423 | } 424 | 425 | // 2D 426 | 427 | inline Vec2f SL::Position0(const Spline2& spline) 428 | { 429 | return Vec2f(spline.xb.x, spline.yb.x); 430 | } 431 | inline Vec2f SL::Position1(const Spline2& spline) 432 | { 433 | return Vec2f(spline.xb.w, spline.yb.w); 434 | } 435 | 436 | inline Vec2f SL::Velocity0(const Spline2& spline) 437 | { 438 | return 3.0f * Vec2f 439 | ( 440 | spline.xb.y - spline.xb.x, 441 | spline.yb.y - spline.yb.x 442 | ); 443 | } 444 | inline Vec2f SL::Velocity1(const Spline2& spline) 445 | { 446 | return 3.0f * Vec2f 447 | ( 448 | spline.xb.w - spline.xb.z, 449 | spline.yb.w - spline.yb.z 450 | ); 451 | } 452 | 453 | inline void SL::ClampAgent(int* index, float* t, int numSplines) 454 | { 455 | if (*index < 0) 456 | { 457 | *index = 0; 458 | *t = 0.0f; 459 | } 460 | else if (*index >= numSplines) 461 | { 462 | *index = numSplines - 1; 463 | *t = 1.0f; 464 | } 465 | else if (*t < 0.0f) 466 | *t = 0.0f; 467 | else if (*t > 1.0f) 468 | *t = 1.0f; 469 | } 470 | 471 | inline void SL::WrapAgent(int* indexInOut, float* tInOut, int numSplines) 472 | { 473 | int& index = *indexInOut; 474 | float& t = *tInOut; 475 | 476 | SL_ASSERT(!IsNAN(t)); 477 | SL_ASSERT(index == 0 || index == numSplines - 1); 478 | 479 | t -= floorf(t); 480 | index ^= numSplines - 1; 481 | } 482 | 483 | inline void SL::ReverseAgent(int* , float* t) 484 | { 485 | *t = ceilf(*t) - *t; 486 | } 487 | 488 | // 3D 489 | 490 | inline Vec3f SL::Position0(const Spline3& spline) 491 | { 492 | return Vec3f(spline.xb.x, spline.yb.x, spline.zb.x); 493 | } 494 | inline Vec3f SL::Position1(const Spline3& spline) 495 | { 496 | return Vec3f(spline.xb.w, spline.yb.w, spline.zb.w); 497 | } 498 | 499 | inline Vec3f SL::Velocity0(const Spline3& spline) 500 | { 501 | return 3.0f * Vec3f 502 | ( 503 | spline.xb.y - spline.xb.x, 504 | spline.yb.y - spline.yb.x, 505 | spline.zb.y - spline.zb.x 506 | ); 507 | } 508 | inline Vec3f SL::Velocity1(const Spline3& spline) 509 | { 510 | return 3.0f * Vec3f 511 | ( 512 | spline.xb.w - spline.xb.z, 513 | spline.yb.w - spline.yb.z, 514 | spline.zb.w - spline.zb.z 515 | ); 516 | } 517 | 518 | #endif 519 | --------------------------------------------------------------------------------