Source/System/Dynamics/VRMLAnimation/Helper/OSGInterpolationHelper.inl

   1 /*---------------------------------------------------------------------------*\
   2  *                                OpenSG                                     *
   3  *                                                                           *
   4  *                                                                           *
   5  *           Copyright (C) 2008 by the OpenSG Forum                          *
   6  *                                                                           *
   7  *                            www.opensg.org                                 *
   8  *                                                                           *
   9  *   contact: dirk@opensg.org, gerrit.voss@vossg.org, jbehr@zgdv.de          *
  10  *                                                                           *
  11 \*---------------------------------------------------------------------------*/
  12 /*---------------------------------------------------------------------------*\
  13  *                                License                                    *
  14  *                                                                           *
  15  * This library is free software; you can redistribute it and/or modify it   *
  16  * under the terms of the GNU Library General Public License as published    *
  17  * by the Free Software Foundation, version 2.                               *
  18  *                                                                           *
  19  * This library is distributed in the hope that it will be useful, but       *
  20  * WITHOUT ANY WARRANTY; without even the implied warranty of                *
  21  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU         *
  22  * Library General Public License for more details.                          *
  23  *                                                                           *
  24  * You should have received a copy of the GNU Library General Public         *
  25  * License along with this library; if not, write to the Free Software       *
  26  * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.                 *
  27  *                                                                           *
  28 \*---------------------------------------------------------------------------*/
  29 /*---------------------------------------------------------------------------*\
  30  *                                Changes                                    *
  31  *                                                                           *
  32  *                                                                           *
  33  *                                                                           *
  34  *                                                                           *
  35  *                                                                           *
  36  *                                                                           *
  37 \*---------------------------------------------------------------------------*/
  38
  39 OSG_BEGIN_NAMESPACE
  40
  41 template<class KeyFieldT, class KeyValueFieldT, class ValueFieldT> inline
  42 void InterpolationHelper<KeyFieldT,
  43                          KeyValueFieldT,
  44                          ValueFieldT>::copyFirstValue(
  45                              const KeyFieldT      &,
  46                              const KeyValueFieldT &mfKeyValues,
  47                                    ValueFieldT    &fValue     )
  48 {
  49     fValue.setValue(mfKeyValues.front());
  50 }
  51
  52 template<class KeyFieldT, class KeyValueFieldT, class ValueFieldT> inline
  53 void InterpolationHelper<KeyFieldT,
  54                          KeyValueFieldT,
  55                          ValueFieldT>::copyLastValue (
  56                              const KeyFieldT      &,
  57                              const KeyValueFieldT &mfKeyValues,
  58                                    ValueFieldT    &fValue     )
  59 {
  60     fValue.setValue(mfKeyValues.back());
  61 }
  62
  63 // Orientation
  64
  65 template<> inline
  66 void InterpolationHelper<MFReal32,
  67                          MFQuaternion,
  68                          SFQuaternion>::lerp(
  69                              const UInt32        uiStopIndex,
  70                              const UInt32        uiStartIndex,
  71                              const Real32        rFraction,
  72                              const MFReal32     &mfKeys,
  73                              const MFQuaternion &mfKeyValues,
  74                                    SFQuaternion &fValue  )
  75 {
  76     if(osgAbs(mfKeys[uiStopIndex] - mfKeys[uiStartIndex]) <
  77        TypeTraits<Real32>::getDefaultEps()                  )
  78     {
  79         return;
  80     }
  81     else
  82     {
  83         Quaternion result;
  84
  85         Real32 t =
  86             (rFraction           - mfKeys[uiStartIndex]) /
  87             (mfKeys[uiStopIndex] - mfKeys[uiStartIndex]);
  88
  89         result.slerpThis(mfKeyValues[uiStartIndex],
  90                          mfKeyValues[uiStopIndex],
  91                          t);
  92
  93         fValue.setValue(result);
  94     }
  95 }
  96
  97 // Position
  98
  99 template<> inline
 100 void InterpolationHelper<MFReal32,
 101                          MFVec3f,
 102                          SFVec3f>::lerp(
 103                              const UInt32    uiStopIndex,
 104                              const UInt32    uiStartIndex,
 105                              const Real32    rFraction,
 106                              const MFReal32 &mfKeys,
 107                              const MFVec3f  &mfKeyValues,
 108                                    SFVec3f  &fValue  )
 109 {
 110     if(osgAbs(mfKeys[uiStopIndex] - mfKeys[uiStartIndex]) <
 111        TypeTraits<Real32>::getDefaultEps()                  )
 112     {
 113         return;
 114     }
 115     else
 116     {
 117         Vec3f vResult;
 118
 119         Real32 t =
 120             (rFraction           - mfKeys[uiStartIndex]) /
 121             (mfKeys[uiStopIndex] - mfKeys[uiStartIndex]);
 122
 123         vResult  = mfKeyValues[uiStopIndex ];
 124         vResult -= mfKeyValues[uiStartIndex];
 125         vResult *= t;
 126
 127         vResult += mfKeyValues[uiStartIndex];
 128
 129         fValue.setValue(vResult);
 130     }
 131 }
 132
 133
 134 // Coordinate
 135
 136 template<> inline
 137 void InterpolationHelper<MFReal32,
 138                          MFPnt3f,
 139                          MFPnt3f>::copyFirstValue(
 140                              const MFReal32 &mfKeys,
 141                              const MFPnt3f  &mfKeyValues,
 142                                    MFPnt3f  &fValue     )
 143 {
 144     UInt32 uiNumPoints = UInt32(mfKeyValues.size() / mfKeys.size());
 145
 146     MFPnt3f::const_iterator startIt = mfKeyValues.begin();
 147     MFPnt3f::const_iterator stopIt  = startIt + uiNumPoints;
 148
 149     fValue.clear();
 150     fValue.insert(fValue.begin(), startIt, stopIt);
 151 }
 152
 153 template<> inline
 154 void InterpolationHelper<MFReal32,
 155                          MFPnt3f,
 156                          MFPnt3f>::copyLastValue (
 157                              const MFReal32 &mfKeys,
 158                              const MFPnt3f  &mfKeyValues,
 159                                    MFPnt3f  &fValue     )
 160 {
 161     UInt32 uiNumPoints = UInt32(mfKeyValues.size() / mfKeys.size());
 162
 163     MFPnt3f::const_iterator stopIt  = mfKeyValues.end();
 164     MFPnt3f::const_iterator startIt = stopIt - uiNumPoints;
 165
 166     fValue.clear();
 167     fValue.insert(fValue.begin(), startIt, stopIt);
 168 }
 169
 170 template<> inline
 171 void InterpolationHelper<MFReal32,
 172                          MFPnt3f,
 173                          MFPnt3f>::lerp(
 174                              const UInt32    uiStopIndex,
 175                              const UInt32    uiStartIndex,
 176                              const Real32    rFraction,
 177                              const MFReal32 &mfKeys,
 178                              const MFPnt3f  &mfKeyValues,
 179                                    MFPnt3f  &fValue  )
 180 {
 181     if(osgAbs(mfKeys[uiStopIndex] - mfKeys[uiStartIndex]) <
 182        TypeTraits<Real32>::getDefaultEps()                  )
 183     {
 184         return;
 185     }
 186     else
 187     {
 188         Pnt3f vResult;
 189
 190         UInt32 uiNumPoints = UInt32(mfKeyValues.size() / mfKeys.size());
 191
 192         Real32 t =
 193             (rFraction           - mfKeys[uiStartIndex]) /
 194             (mfKeys[uiStopIndex] - mfKeys[uiStartIndex]);
 195
 196         UInt32 uiIndex1    = uiStartIndex * uiNumPoints;
 197         UInt32 uiIndex2    = uiStopIndex  * uiNumPoints;
 198
 199         fValue.clear();
 200
 201         for(UInt32 i = 0; i < uiNumPoints; i++)
 202         {
 203             vResult  = mfKeyValues[uiIndex2];
 204             vResult -= mfKeyValues[uiIndex1].subZero();
 205             vResult *= t;
 206
 207             vResult += mfKeyValues[uiIndex1].subZero();
 208
 209             fValue.push_back(vResult);
 210
 211             ++uiIndex1;
 212             ++uiIndex2;
 213         }
 214     }
 215 }
 216
 217
 218 // Normal
 219
 220 template<> inline
 221 void InterpolationHelper<MFReal32,
 222                          MFVec3f,
 223                          MFVec3f>::copyFirstValue(
 224                              const MFReal32 &mfKeys,
 225                              const MFVec3f  &mfKeyValues,
 226                                    MFVec3f  &fValue     )
 227 {
 228     UInt32 uiNumPoints = UInt32(mfKeyValues.size() / mfKeys.size());
 229
 230     MFVec3f::const_iterator startIt = mfKeyValues.begin();
 231     MFVec3f::const_iterator stopIt  = startIt + uiNumPoints;
 232
 233     fValue.clear();
 234     fValue.insert(fValue.begin(), startIt, stopIt);
 235 }
 236
 237 template<> inline
 238 void InterpolationHelper<MFReal32,
 239                          MFVec3f,
 240                          MFVec3f>::copyLastValue (
 241                              const MFReal32 &mfKeys,
 242                              const MFVec3f  &mfKeyValues,
 243                                    MFVec3f  &fValue     )
 244 {
 245     UInt32 uiNumPoints = UInt32(mfKeyValues.size() / mfKeys.size());
 246
 247     MFVec3f::const_iterator stopIt  = mfKeyValues.end();
 248     MFVec3f::const_iterator startIt = stopIt - uiNumPoints;
 249
 250     fValue.clear();
 251     fValue.insert(fValue.begin(), startIt, stopIt);
 252 }
 253
 254 template<> inline
 255 void InterpolationHelper<MFReal32,
 256                          MFVec3f,
 257                          MFVec3f>::lerp(
 258                              const UInt32    uiStopIndex,
 259                              const UInt32    uiStartIndex,
 260                              const Real32    rFraction,
 261                              const MFReal32 &mfKeys,
 262                              const MFVec3f  &mfKeyValues,
 263                                    MFVec3f  &fValue  )
 264 {
 265     if(osgAbs(mfKeys[uiStopIndex] - mfKeys[uiStartIndex]) <
 266        TypeTraits<Real32>::getDefaultEps()                  )
 267     {
 268         return;
 269     }
 270     else
 271     {
 272         Vec3f vResult;
 273
 274         UInt32 uiNumPoints = UInt32(mfKeyValues.size() / mfKeys.size());
 275
 276         Real32 t =
 277             (rFraction           - mfKeys[uiStartIndex]) /
 278             (mfKeys[uiStopIndex] - mfKeys[uiStartIndex]);
 279
 280         UInt32 uiIndex1    = uiStartIndex * uiNumPoints;
 281         UInt32 uiIndex2    = uiStopIndex  * uiNumPoints;
 282
 283         fValue.clear();
 284
 285         for(UInt32 i = 0; i < uiNumPoints; i++)
 286         {
 287             vResult  = mfKeyValues[uiIndex2];
 288             vResult -= mfKeyValues[uiIndex1].subZero();
 289             vResult *= t;
 290
 291             vResult += mfKeyValues[uiIndex1].subZero();
 292
 293             fValue.push_back(vResult);
 294
 295             ++uiIndex1;
 296             ++uiIndex2;
 297         }
 298     }
 299 }
 300
 301
 302 // Scalar
 303
 304 template<> inline
 305 void InterpolationHelper<MFReal32,
 306                          MFReal32,
 307                          SFReal32>::lerp(
 308                              const UInt32    uiStopIndex,
 309                              const UInt32    uiStartIndex,
 310                              const Real32    rFraction,
 311                              const MFReal32 &mfKeys,
 312                              const MFReal32 &mfKeyValues,
 313                                    SFReal32 &fValue  )
 314 {
 315     if(osgAbs(mfKeys[uiStopIndex] - mfKeys[uiStartIndex]) <
 316        TypeTraits<Real32>::getDefaultEps()                  )
 317     {
 318         return;
 319     }
 320     else
 321     {
 322         Real32 vResult;
 323
 324         Real32 t =
 325             (rFraction           - mfKeys[uiStartIndex]) /
 326             (mfKeys[uiStopIndex] - mfKeys[uiStartIndex]);
 327
 328         vResult  = mfKeyValues[uiStopIndex ];
 329         vResult -= mfKeyValues[uiStartIndex];
 330         vResult *= t;
 331
 332         vResult += mfKeyValues[uiStartIndex];
 333
 334         fValue.setValue(vResult);
 335     }
 336 }
 337
 338
 339 // Interpolate
 340
 341 template<class KeyFieldT, class KeyValueFieldT, class ValueFieldT> inline
 342 void InterpolationHelper<KeyFieldT,
 343                          KeyValueFieldT,
 344                          ValueFieldT>::interpolate(
 345                              const Real32          rFraction,
 346                              const KeyFieldT      &mfKeys,
 347                              const KeyValueFieldT &mfKeyValues,
 348                                    ValueFieldT    &fValue     )
 349 {
 350     typename KeyFieldT::const_iterator keyIt;
 351
 352     if(mfKeys.size() == 0 || mfKeyValues.size() == 0)
 353     {
 354         return;
 355     }
 356
 357     keyIt = lower_bound(mfKeys.begin(),
 358                         mfKeys.end  (),
 359                         rFraction);
 360
 361     if(keyIt != mfKeys.end())
 362     {
 363         if(keyIt == mfKeys.begin())
 364         {
 365             copyFirstValue(mfKeys, mfKeyValues, fValue);
 366         }
 367         else
 368         {
 369             UInt32 uiStopIndex  = keyIt - mfKeys.begin();
 370             UInt32 uiStartIndex = uiStopIndex - 1;
 371
 372             lerp(uiStartIndex,
 373                  uiStopIndex,
 374                  rFraction,
 375                  mfKeys,
 376                  mfKeyValues,
 377                  fValue     );
 378         }
 379     }
 380     else
 381     {
 382         copyLastValue(mfKeys, mfKeyValues, fValue);
 383     }
 384 }
 385
 386 template<class KeyFieldT, class KeyValueFieldT, class ValueFieldT>
 387 template<class ResortIndexTypeT> inline
 388 void InterpolationHelper<KeyFieldT,
 389                          KeyValueFieldT,
 390                          ValueFieldT>::resortKeyValues(
 391                                    SizeT             uiNumKeys,
 392                                    SizeT             uiValuesPerKey,
 393                                    KeyValueFieldT   &mfKeyValues,
 394                              const ResortIndexTypeT &mfResortIndex)
 395 {
 396     KeyValueFieldT tmpKeyValues;
 397
 398     tmpKeyValues.resize(mfKeyValues.size());
 399
 400     SizeT uiGlobalIdx = 0;
 401
 402     for(SizeT i = 0; i < uiNumKeys; ++i)
 403     {
 404         SizeT uiGlobalBase = i * uiValuesPerKey;
 405
 406         for(SizeT j = 0; j < uiValuesPerKey; ++j, ++uiGlobalIdx)
 407         {
 408             tmpKeyValues[uiGlobalIdx] =
 409                 mfKeyValues[uiGlobalBase + mfResortIndex[j]];
 410         }
 411     }
 412
 413     mfKeyValues = tmpKeyValues;
 414 }
 415
 416 OSG_END_NAMESPACE