Source/Base/Base/OSGSimpleCurve.cpp

   1 /*---------------------------------------------------------------------------*\
   2  *                                OpenSG                                     *
   3  *                                                                           *
   4  *                                                                           *
   5  *             Copyright (C) 2000-2003 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 /*---------------------------------------------------------------------------*\
  31  *                                Changes                                    *
  32  *                                                                           *
  33  *                                                                           *
  34  *                                                                           *
  35  *                                                                           *
  36  *                                                                           *
  37  *                                                                           *
  38 \*---------------------------------------------------------------------------*/
  39
  40 //---------------------------------------------------------------------------
  41 //  Includes
  42 //---------------------------------------------------------------------------
  43
  44 #include <boost/foreach.hpp>
  45 #include <boost/tuple/tuple.hpp>
  46 #include "boost/tuple/tuple_comparison.hpp"
  47
  48 #include "OSGConfig.h"
  49 #include "OSGSimpleCurve.h"
  50
  51 OSG_BEGIN_NAMESPACE
  52
  53 CubicBezierCurve::CubicBezierCurve(
  54     const OSG::Pnt3f& p0,
  55     const OSG::Pnt3f& p1,
  56     const OSG::Pnt3f& p2,
  57     const OSG::Pnt3f& p3)
  58 {
  59     p[0] = p0; p[1] = p1; p[2] = p2; p[3] = p3;
  60 }
  61
  62 CubicBezierCurve::CubicBezierCurve(const CubicBezierCurve& rhs)
  63 {
  64     p[0] = rhs.p[0]; p[1] = rhs.p[1]; p[2] = rhs.p[2]; p[3] = rhs.p[3];
  65 }
  66
  67 OSG::Vec3f CubicBezierCurve::getBinormal(OSG::Real32 t) const
  68 {
  69     OSG::Vec3f binormal = fst_derivative(t);
  70     binormal.crossThis(sec_derivative(t));
  71     binormal.normalize();
  72
  73     while (OSG::osgAbs(binormal.length() - 1.f) >= OSG::Eps)
  74     {
  75         if (t + OSG::Eps <= 1.f)
  76             t += OSG::Eps;
  77         else if (t - OSG::Eps >= 0.f)
  78             t -= OSG::Eps;
  79         else
  80             OSG_ASSERT(false);
  81
  82         binormal = fst_derivative(t);
  83         binormal.crossThis(sec_derivative(t));
  84         binormal.normalize();
  85     }
  86
  87     return binormal;
  88 }
  89
  90 OSG::Matrix CubicBezierCurve::getFrame(OSG::Real32 t, bool position_only) const
  91 {
  92     OSG::Matrix mat;
  93     mat.identity();
  94
  95     OSG::Vec3f P = (*this)(t).subZero();
  96
  97     if (position_only)
  98     {
  99         mat.setTranslate(P);
 100     }
 101     else
 102     {
 103         OSG::Vec3f T = getTangent(t);
 104         OSG::Vec3f N = getNormal(t);
 105         OSG::Vec3f B = getBinormal(t);
 106
 107 #if 0
 108         OSG::Real32 lT = T.length();
 109         OSG::Real32 lN = N.length();
 110         OSG::Real32 lB = B.length();
 111
 112         OSG::Real32 v1 = T.dot(N);
 113         OSG::Real32 v2 = T.dot(B);
 114         OSG::Real32 v3 = N.dot(B);
 115 #endif
 116
 117         OSG_ASSERT(OSG::osgAbs(T.length() - 1.f) < OSG::Eps);
 118         OSG_ASSERT(OSG::osgAbs(N.length() - 1.f) < OSG::Eps);
 119         OSG_ASSERT(OSG::osgAbs(B.length() - 1.f) < OSG::Eps);
 120
 121         mat.setValue(T,N,B,P);
 122     }
 123
 124     return mat;
 125 }
 126
 127 OSG::Real32 CubicBezierCurve::getLength(OSG::UInt32 numSeg) const
 128 {
 129     OSG_ASSERT(numSeg >= 2);
 130
 131     OSG::Real32 d = 1.f / numSeg;
 132     OSG::Real32 t = d;
 133     OSG::Real32 l = 0.f;
 134
 135     OSG::Pnt3f p0 = operator()(0.f);
 136
 137     while (t <= 1.f)
 138     {
 139         OSG::Pnt3f p1 = operator()(t);
 140
 141         l += (p1 - p0).length();
 142         t += d;
 143     }
 144
 145     return l;
 146 }
 147
 148 OSG::Vec3f CubicBezierCurve::fst_derivative(OSG::Real32 t) const
 149 {
 150     OSG::Vec3f v10 = p[1] - p[0];
 151     OSG::Vec3f v21 = p[2] - p[1];
 152     OSG::Vec3f v32 = p[3] - p[2];
 153
 154     OSG::Real32    e = 1.f - t;
 155     OSG::Real32 sq_e = e * e;
 156     OSG::Real32 sq_t = t * t;
 157
 158     return 3.f * sq_e * v10 + 6.f * e * t * v21 + 3.f * sq_t * v32;
 159 }
 160
 161 OSG::Vec3f CubicBezierCurve::sec_derivative(OSG::Real32 t) const
 162 {
 163     OSG::Vec3f v210 = p[2] - 2.f * p[1] + p[0].subZero();
 164     OSG::Vec3f v321 = p[3] - 2.f * p[2] + p[1].subZero();
 165
 166     OSG::Real32 e = 1.f - t;
 167
 168     return 6.f * e * v210 + 6.f * t * v321;
 169 }
 170
 171 OSG::Vec3f CubicBezierCurve::thr_devivative(OSG::Real32 t) const
 172 {
 173     OSG::Vec3f v21 = p[2] - p[1];
 174     OSG::Vec3f v30 = p[3] - p[0];
 175
 176     return -18.f * v21 + 6.f * v30;
 177 }
 178
 179
 180 SmoothCubicBezierSpline::SmoothCubicBezierSpline(const std::vector<OSG::Pnt3f>& vKnots)
 181 : knots(vKnots)
 182 , intervals()
 183 , curves()
 184 {
 185     OSG_ASSERT(knots.size() > 3);
 186
 187     points_t p1, p2;
 188
 189     calc_ctrl_pnts(p1, p2);
 190
 191     std::size_t n = knots.size() - 1;
 192
 193     std::vector<OSG::Real32> lengths(n);
 194     OSG::Real32 L = 0.f;
 195
 196     for (std::size_t i = 0; i < n; ++i)
 197     {
 198         curves.push_back(CubicBezierCurve(knots[i], p1[i], p2[i], knots[i+1]));
 199
 200         lengths[i] = curves[i].getLength();
 201         L += lengths[i];
 202     }
 203
 204     OSG_ASSERT(L > 0.f);
 205
 206     OSG::Real32 d = 1.f / L;
 207
 208     intervals.resize(n);
 209
 210     intervals[0] = d * lengths[0];
 211
 212     for (std::size_t i = 1; i < n-1; ++i)
 213     {
 214         intervals[i] = intervals[i-1] + d * lengths[i];
 215     }
 216
 217     intervals[n-1] = 1.f;
 218 }
 219
 220 SmoothCubicBezierSpline::SmoothCubicBezierSpline(const SmoothCubicBezierSpline& rhs)
 221 : knots(rhs.knots)
 222 , intervals(rhs.intervals)
 223 , curves(rhs.curves)
 224 {
 225 }
 226
 227 SmoothCubicBezierSpline& SmoothCubicBezierSpline::operator=(const SmoothCubicBezierSpline& rhs)
 228 {
 229     if (this != &rhs)
 230     {
 231         knots     = rhs.knots;
 232         intervals = rhs.intervals;
 233         curves    = rhs.curves;
 234     }
 235     return *this;
 236 }
 237
 238 OSG::Real32 SmoothCubicBezierSpline::getLength(OSG::UInt32 numSeg) const
 239 {
 240     OSG::Real32 l = 0.f;
 241     BOOST_FOREACH(const CubicBezierCurve& c, curves)
 242     {
 243         l += c.getLength(numSeg);
 244     }
 245     return l;
 246 }
 247
 248 void SmoothCubicBezierSpline::calc_ctrl_pnts(points_t& p1, points_t& p2) const
 249 {
 250     namespace tp = boost::tuples;
 251     typedef tp::tuple<OSG::Real32, OSG::Real32, OSG::Real32> tuple_t;
 252     typedef std::vector<tuple_t> tuples_t;
 253
 254     std::size_t n = knots.size()-1;
 255
 256     p1.resize(n);
 257     p2.resize(n);
 258
 259     tuples_t a(n), b(n), c(n), d(n);
 260
 261     a[0] = tp::make_tuple(0.f, 0.f, 0.f);
 262     b[0] = tp::make_tuple(2.f, 2.f, 2.f);
 263     c[0] = tp::make_tuple(1.f, 1.f, 1.f);
 264     d[0] = tp::make_tuple(
 265         knots[0].x() + 2.f * knots[1].x(),
 266         knots[0].y() + 2.f * knots[1].y(),
 267         knots[0].z() + 2.f * knots[1].z());
 268
 269     for (std::size_t i = 1; i < n-1; ++i)
 270     {
 271         a[i] = tp::make_tuple(1.f, 1.f, 1.f);
 272         b[i] = tp::make_tuple(4.f, 4.f, 4.f);
 273         c[i] = tp::make_tuple(1.f, 1.f, 1.f);
 274         d[i] = tp::make_tuple(
 275             4.f * knots[i].x() + 2.f * knots[i+1].x(),
 276             4.f * knots[i].y() + 2.f * knots[i+1].y(),
 277             4.f * knots[i].z() + 2.f * knots[i+1].z());
 278     }
 279
 280     a[n-1] = tp::make_tuple(2.f, 2.f, 2.f);
 281     b[n-1] = tp::make_tuple(7.f, 7.f, 7.f);
 282     c[n-1] = tp::make_tuple(0.f, 0.f, 0.f);
 283     d[n-1] = tp::make_tuple(
 284         8.f * knots[n-1].x() + 2.f * knots[n].x(),
 285         8.f * knots[n-1].y() + 2.f * knots[n].y(),
 286         8.f * knots[n-1].z() + 2.f * knots[n].z());
 287
 288     for (std::size_t i = 1; i < n; ++i)
 289     {
 290         tuple_t m = tp::make_tuple(
 291             a[i].get<0>() / b[i-1].get<0>(),
 292             a[i].get<1>() / b[i-1].get<1>(),
 293             a[i].get<2>() / b[i-1].get<2>());
 294
 295             b[i] = tp::make_tuple(
 296                 b[i].get<0>() - m.get<0>() * c[i-1].get<0>(),
 297                 b[i].get<1>() - m.get<1>() * c[i-1].get<1>(),
 298                 b[i].get<2>() - m.get<2>() * c[i-1].get<2>());
 299
 300             d[i] = tp::make_tuple(
 301                 d[i].get<0>() - m.get<0>() * d[i-1].get<0>(),
 302                 d[i].get<1>() - m.get<1>() * d[i-1].get<1>(),
 303                 d[i].get<2>() - m.get<2>() * d[i-1].get<2>());
 304     }
 305
 306     p1[n-1].setValues(
 307         d[n-1].get<0>() / b[n-1].get<0>(),
 308         d[n-1].get<1>() / b[n-1].get<1>(),
 309         d[n-1].get<2>() / b[n-1].get<2>());
 310
 311     for (long long i = n-2; i >= 0; --i)
 312     {
 313         p1[i].setValues(
 314             (d[i].get<0>() - c[i].get<0>() * p1[i+1].x()) / b[i].get<0>(),
 315             (d[i].get<1>() - c[i].get<1>() * p1[i+1].y()) / b[i].get<1>(),
 316             (d[i].get<2>() - c[i].get<2>() * p1[i+1].z()) / b[i].get<2>());
 317     }
 318
 319     for (std::size_t i = 0; i < n-1; ++i)
 320     {
 321         p2[i].setValues(
 322             2.f * knots[i+1].x() - p1[i+1].x(),
 323             2.f * knots[i+1].y() - p1[i+1].y(),
 324             2.f * knots[i+1].z() - p1[i+1].z());
 325     }
 326
 327     p2[n-1].setValues(
 328             0.5f * knots[n].x() - p1[n-1].x(),
 329             0.5f * knots[n].y() - p1[n-1].y(),
 330             0.5f * knots[n].z() - p1[n-1].z());
 331 }
 332
 333 OSG_END_NAMESPACE