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