1 /*
2 -----------------------------------------------------------------------------
3 This source file is part of OGRE
4     (Object-oriented Graphics Rendering Engine)
5 For the latest info, see http://www.ogre3d.org/
6 
7 Copyright (c) 2000-2013 Torus Knot Software Ltd
8 
9 Permission is hereby granted, free of charge, to any person obtaining a copy
10 of this software and associated documentation files (the "Software"), to deal
11 in the Software without restriction, including without limitation the rights
12 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
13 copies of the Software, and to permit persons to whom the Software is
14 furnished to do so, subject to the following conditions:
15 
16 The above copyright notice and this permission notice shall be included in
17 all copies or substantial portions of the Software.
18 
19 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
20 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
21 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
22 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
23 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
24 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
25 THE SOFTWARE.
26 -----------------------------------------------------------------------------
27 */
28 #include "OgreStableHeaders.h"
29 #include "OgreSimpleSpline.h"
30 #include "OgreVector4.h"
31 #include "OgreMatrix4.h"
32 
33 
34 
35 namespace Ogre {
36 
37     //---------------------------------------------------------------------
SimpleSpline()38     SimpleSpline::SimpleSpline()
39     {
40         // Set up matrix
41         // Hermite polynomial
42         mCoeffs[0][0] = 2;
43         mCoeffs[0][1] = -2;
44         mCoeffs[0][2] = 1;
45         mCoeffs[0][3] = 1;
46         mCoeffs[1][0] = -3;
47         mCoeffs[1][1] = 3;
48         mCoeffs[1][2] = -2;
49         mCoeffs[1][3] = -1;
50         mCoeffs[2][0] = 0;
51         mCoeffs[2][1] = 0;
52         mCoeffs[2][2] = 1;
53         mCoeffs[2][3] = 0;
54         mCoeffs[3][0] = 1;
55         mCoeffs[3][1] = 0;
56         mCoeffs[3][2] = 0;
57         mCoeffs[3][3] = 0;
58 
59         mAutoCalc = true;
60     }
61     //---------------------------------------------------------------------
~SimpleSpline()62     SimpleSpline::~SimpleSpline()
63     {
64     }
65     //---------------------------------------------------------------------
addPoint(const Vector3 & p)66     void SimpleSpline::addPoint(const Vector3& p)
67     {
68         mPoints.push_back(p);
69         if (mAutoCalc)
70         {
71             recalcTangents();
72         }
73     }
74     //---------------------------------------------------------------------
interpolate(Real t) const75     Vector3 SimpleSpline::interpolate(Real t) const
76     {
77         // Currently assumes points are evenly spaced, will cause velocity
78         // change where this is not the case
79         // TODO: base on arclength?
80 
81 
82         // Work out which segment this is in
83         Real fSeg = t * (mPoints.size() - 1);
84         unsigned int segIdx = (unsigned int)fSeg;
85         // Apportion t
86         t = fSeg - segIdx;
87 
88         return interpolate(segIdx, t);
89 
90     }
91     //---------------------------------------------------------------------
interpolate(unsigned int fromIndex,Real t) const92     Vector3 SimpleSpline::interpolate(unsigned int fromIndex, Real t) const
93     {
94         // Bounds check
95         assert (fromIndex < mPoints.size() &&
96             "fromIndex out of bounds");
97 
98         if ((fromIndex + 1) == mPoints.size())
99         {
100             // Duff request, cannot blend to nothing
101             // Just return source
102             return mPoints[fromIndex];
103 
104         }
105 
106         // Fast special cases
107         if (t == 0.0f)
108         {
109             return mPoints[fromIndex];
110         }
111         else if(t == 1.0f)
112         {
113             return mPoints[fromIndex + 1];
114         }
115 
116         // Real interpolation
117         // Form a vector of powers of t
118         Real t2, t3;
119         t2 = t * t;
120         t3 = t2 * t;
121         Vector4 powers(t3, t2, t, 1);
122 
123 
124         // Algorithm is ret = powers * mCoeffs * Matrix4(point1, point2, tangent1, tangent2)
125         const Vector3& point1 = mPoints[fromIndex];
126         const Vector3& point2 = mPoints[fromIndex+1];
127         const Vector3& tan1 = mTangents[fromIndex];
128         const Vector3& tan2 = mTangents[fromIndex+1];
129         Matrix4 pt;
130 
131         pt[0][0] = point1.x;
132         pt[0][1] = point1.y;
133         pt[0][2] = point1.z;
134         pt[0][3] = 1.0f;
135         pt[1][0] = point2.x;
136         pt[1][1] = point2.y;
137         pt[1][2] = point2.z;
138         pt[1][3] = 1.0f;
139         pt[2][0] = tan1.x;
140         pt[2][1] = tan1.y;
141         pt[2][2] = tan1.z;
142         pt[2][3] = 1.0f;
143         pt[3][0] = tan2.x;
144         pt[3][1] = tan2.y;
145         pt[3][2] = tan2.z;
146         pt[3][3] = 1.0f;
147 
148         Vector4 ret = powers * mCoeffs * pt;
149 
150 
151         return Vector3(ret.x, ret.y, ret.z);
152 
153 
154 
155 
156     }
157     //---------------------------------------------------------------------
recalcTangents(void)158     void SimpleSpline::recalcTangents(void)
159     {
160         // Catmull-Rom approach
161         //
162         // tangent[i] = 0.5 * (point[i+1] - point[i-1])
163         //
164         // Assume endpoint tangents are parallel with line with neighbour
165 
166         size_t i, numPoints;
167         bool isClosed;
168 
169         numPoints = mPoints.size();
170         if (numPoints < 2)
171         {
172             // Can't do anything yet
173             return;
174         }
175 
176         // Closed or open?
177         if (mPoints[0] == mPoints[numPoints-1])
178         {
179             isClosed = true;
180         }
181         else
182         {
183             isClosed = false;
184         }
185 
186         mTangents.resize(numPoints);
187 
188 
189 
190         for(i = 0; i < numPoints; ++i)
191         {
192             if (i ==0)
193             {
194                 // Special case start
195                 if (isClosed)
196                 {
197                     // Use numPoints-2 since numPoints-1 is the last point and == [0]
198                     mTangents[i] = 0.5 * (mPoints[1] - mPoints[numPoints-2]);
199                 }
200                 else
201                 {
202                     mTangents[i] = 0.5 * (mPoints[1] - mPoints[0]);
203                 }
204             }
205             else if (i == numPoints-1)
206             {
207                 // Special case end
208                 if (isClosed)
209                 {
210                     // Use same tangent as already calculated for [0]
211                     mTangents[i] = mTangents[0];
212                 }
213                 else
214                 {
215                     mTangents[i] = 0.5 * (mPoints[i] - mPoints[i-1]);
216                 }
217             }
218             else
219             {
220                 mTangents[i] = 0.5 * (mPoints[i+1] - mPoints[i-1]);
221             }
222 
223         }
224 
225 
226 
227     }
228     //---------------------------------------------------------------------
getPoint(unsigned short index) const229     const Vector3& SimpleSpline::getPoint(unsigned short index) const
230     {
231         assert (index < mPoints.size() && "Point index is out of bounds!!");
232 
233         return mPoints[index];
234     }
235     //---------------------------------------------------------------------
getNumPoints(void) const236     unsigned short SimpleSpline::getNumPoints(void) const
237     {
238         return (unsigned short)mPoints.size();
239     }
240     //---------------------------------------------------------------------
clear(void)241     void SimpleSpline::clear(void)
242     {
243         mPoints.clear();
244         mTangents.clear();
245     }
246     //---------------------------------------------------------------------
updatePoint(unsigned short index,const Vector3 & value)247     void SimpleSpline::updatePoint(unsigned short index, const Vector3& value)
248     {
249         assert (index < mPoints.size() && "Point index is out of bounds!!");
250 
251         mPoints[index] = value;
252         if (mAutoCalc)
253         {
254             recalcTangents();
255         }
256     }
257     //---------------------------------------------------------------------
setAutoCalculate(bool autoCalc)258     void SimpleSpline::setAutoCalculate(bool autoCalc)
259     {
260         mAutoCalc = autoCalc;
261     }
262 
263 
264 
265 
266 }
267 
268 
269 
270 
271