src/GeomEvaluator/GeomEvaluator_OffsetCurve.cxx

   1 // Created on: 2015-09-21
   2 // Copyright (c) 2015 OPEN CASCADE SAS
   3 //
   4 // This file is part of Open CASCADE Technology software library.
   5 //
   6 // This library is free software; you can redistribute it and/or modify it under
   7 // the terms of the GNU Lesser General Public License version 2.1 as published
   8 // by the Free Software Foundation, with special exception defined in the file
   9 // OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
  10 // distribution for complete text of the license and disclaimer of any warranty.
  11 //
  12 // Alternatively, this file may be used under the terms of Open CASCADE
  13 // commercial license or contractual agreement.
  14
  15 #include <GeomEvaluator_OffsetCurve.hxx>
  16
  17 #include <GeomAdaptor_HCurve.hxx>
  18 #include <Standard_NullValue.hxx>
  19
  20
  21 IMPLEMENT_STANDARD_RTTIEXT(GeomEvaluator_OffsetCurve,GeomEvaluator_Curve)
  22
  23 GeomEvaluator_OffsetCurve::GeomEvaluator_OffsetCurve(
  24         const Handle(Geom_Curve)& theBase,
  25         const Standard_Real theOffset,
  26         const gp_Dir& theDirection)
  27   : GeomEvaluator_Curve(),
  28     myBaseCurve(theBase),
  29     myOffset(theOffset),
  30     myOffsetDir(theDirection)
  31 {
  32 }
  33
  34 GeomEvaluator_OffsetCurve::GeomEvaluator_OffsetCurve(
  35         const Handle(GeomAdaptor_HCurve)& theBase,
  36         const Standard_Real theOffset,
  37         const gp_Dir& theDirection)
  38   : GeomEvaluator_Curve(),
  39     myBaseAdaptor(theBase),
  40     myOffset(theOffset),
  41     myOffsetDir(theDirection)
  42 {
  43 }
  44
  45 void GeomEvaluator_OffsetCurve::D0(const Standard_Real theU,
  46                                          gp_Pnt& theValue) const
  47 {
  48   gp_Vec aD1;
  49   BaseD1(theU, theValue, aD1);
  50   CalculateD0(theValue, aD1);
  51 }
  52
  53 void GeomEvaluator_OffsetCurve::D1(const Standard_Real theU,
  54                                          gp_Pnt& theValue,
  55                                          gp_Vec& theD1) const
  56 {
  57   gp_Vec aD2;
  58   BaseD2(theU, theValue, theD1, aD2);
  59   CalculateD1(theValue, theD1, aD2);
  60 }
  61
  62 void GeomEvaluator_OffsetCurve::D2(const Standard_Real theU,
  63                                          gp_Pnt& theValue,
  64                                          gp_Vec& theD1,
  65                                          gp_Vec& theD2) const
  66 {
  67   gp_Vec aD3;
  68   BaseD3(theU, theValue, theD1, theD2, aD3);
  69
  70   Standard_Boolean isDirectionChange = Standard_False;
  71   if (theD1.SquareMagnitude() <= gp::Resolution())
  72   {
  73     gp_Vec aDummyD4;
  74     isDirectionChange = AdjustDerivative(3, theU, theD1, theD2, aD3, aDummyD4);
  75   }
  76
  77   CalculateD2(theValue, theD1, theD2, aD3, isDirectionChange);
  78 }
  79
  80 void GeomEvaluator_OffsetCurve::D3(const Standard_Real theU,
  81                                          gp_Pnt& theValue,
  82                                          gp_Vec& theD1,
  83                                          gp_Vec& theD2,
  84                                          gp_Vec& theD3) const
  85 {
  86   gp_Vec aD4;
  87   BaseD4(theU, theValue, theD1, theD2, theD3, aD4);
  88
  89   Standard_Boolean isDirectionChange = Standard_False;
  90   if (theD1.SquareMagnitude() <= gp::Resolution())
  91     isDirectionChange = AdjustDerivative(4, theU, theD1, theD2, theD3, aD4);
  92
  93   CalculateD3(theValue, theD1, theD2, theD3, aD4, isDirectionChange);
  94 }
  95
  96 gp_Vec GeomEvaluator_OffsetCurve::DN(const Standard_Real theU,
  97                                      const Standard_Integer theDeriv) const
  98 {
  99   Standard_RangeError_Raise_if(theDeriv < 1, "GeomEvaluator_OffsetCurve::DN(): theDeriv < 1");
 100
 101   gp_Pnt aPnt;
 102   gp_Vec aDummy, aDN;
 103   switch (theDeriv)
 104   {
 105   case 1:
 106     D1(theU, aPnt, aDN);
 107     break;
 108   case 2:
 109     D2(theU, aPnt, aDummy, aDN);
 110     break;
 111   case 3:
 112     D3(theU, aPnt, aDummy, aDummy, aDN);
 113     break;
 114   default:
 115     aDN = BaseDN(theU, theDeriv);
 116   }
 117   return aDN;
 118 }
 119
 120
 121 void GeomEvaluator_OffsetCurve::BaseD0(const Standard_Real theU,
 122                                              gp_Pnt& theValue) const
 123 {
 124   if (!myBaseAdaptor.IsNull())
 125     myBaseAdaptor->D0(theU, theValue);
 126   else
 127     myBaseCurve->D0(theU, theValue);
 128 }
 129
 130 void GeomEvaluator_OffsetCurve::BaseD1(const Standard_Real theU,
 131                                              gp_Pnt& theValue,
 132                                              gp_Vec& theD1) const
 133 {
 134   if (!myBaseAdaptor.IsNull())
 135     myBaseAdaptor->D1(theU, theValue, theD1);
 136   else
 137     myBaseCurve->D1(theU, theValue, theD1);
 138 }
 139
 140 void GeomEvaluator_OffsetCurve::BaseD2(const Standard_Real theU,
 141                                              gp_Pnt& theValue,
 142                                              gp_Vec& theD1,
 143                                              gp_Vec& theD2) const
 144 {
 145   if (!myBaseAdaptor.IsNull())
 146     myBaseAdaptor->D2(theU, theValue, theD1, theD2);
 147   else
 148     myBaseCurve->D2(theU, theValue, theD1, theD2);
 149 }
 150
 151 void GeomEvaluator_OffsetCurve::BaseD3(const Standard_Real theU,
 152                                              gp_Pnt& theValue,
 153                                              gp_Vec& theD1,
 154                                              gp_Vec& theD2,
 155                                              gp_Vec& theD3) const
 156 {
 157   if (!myBaseAdaptor.IsNull())
 158     myBaseAdaptor->D3(theU, theValue, theD1, theD2, theD3);
 159   else
 160     myBaseCurve->D3(theU, theValue, theD1, theD2, theD3);
 161 }
 162
 163 void GeomEvaluator_OffsetCurve::BaseD4(const Standard_Real theU,
 164                                              gp_Pnt& theValue,
 165                                              gp_Vec& theD1,
 166                                              gp_Vec& theD2,
 167                                              gp_Vec& theD3,
 168                                              gp_Vec& theD4) const
 169 {
 170   if (!myBaseAdaptor.IsNull())
 171   {
 172     myBaseAdaptor->D3(theU, theValue, theD1, theD2, theD3);
 173     theD4 = myBaseAdaptor->DN(theU, 4);
 174   }
 175   else
 176   {
 177     myBaseCurve->D3(theU, theValue, theD1, theD2, theD3);
 178     theD4 = myBaseCurve->DN(theU, 4);
 179   }
 180 }
 181
 182 gp_Vec GeomEvaluator_OffsetCurve::BaseDN(const Standard_Real theU,
 183                                          const Standard_Integer theDeriv) const
 184 {
 185   if (!myBaseAdaptor.IsNull())
 186     return myBaseAdaptor->DN(theU, theDeriv);
 187   return myBaseCurve->DN(theU, theDeriv);
 188 }
 189
 190
 191 void GeomEvaluator_OffsetCurve::CalculateD0(      gp_Pnt& theValue,
 192                                             const gp_Vec& theD1) const
 193 {
 194   gp_XYZ Ndir = (theD1.XYZ()).Crossed(myOffsetDir.XYZ());
 195   Standard_Real R = Ndir.Modulus();
 196   if (R <= gp::Resolution())
 197     throw Standard_NullValue("GeomEvaluator_OffsetCurve: Undefined normal vector "
 198                               "because tangent vector has zero-magnitude!");
 199
 200   Ndir.Multiply(myOffset / R);
 201   theValue.ChangeCoord().Add(Ndir);
 202 }
 203
 204 void GeomEvaluator_OffsetCurve::CalculateD1(      gp_Pnt& theValue,
 205                                                   gp_Vec& theD1,
 206                                             const gp_Vec& theD2) const
 207 {
 208   // P(u) = p(u) + Offset * Ndir / R
 209   // with R = || p' ^ V|| and Ndir = P' ^ direction (local normal direction)
 210
 211   // P'(u) = p'(u) + (Offset / R**2) * (DNdir/DU * R -  Ndir * (DR/R))
 212
 213   gp_XYZ Ndir = (theD1.XYZ()).Crossed(myOffsetDir.XYZ());
 214   gp_XYZ DNdir = (theD2.XYZ()).Crossed(myOffsetDir.XYZ());
 215   Standard_Real R2 = Ndir.SquareModulus();
 216   Standard_Real R = Sqrt(R2);
 217   Standard_Real R3 = R * R2;
 218   Standard_Real Dr = Ndir.Dot(DNdir);
 219   if (R3 <= gp::Resolution()) {
 220     if (R2 <= gp::Resolution())
 221       throw Standard_NullValue("GeomEvaluator_OffsetCurve: Null derivative");
 222     //We try another computation but the stability is not very good.
 223     DNdir.Multiply(R);
 224     DNdir.Subtract(Ndir.Multiplied(Dr / R));
 225     DNdir.Multiply(myOffset / R2);
 226   }
 227   else {
 228     // Same computation as IICURV in EUCLID-IS because the stability is better
 229     DNdir.Multiply(myOffset / R);
 230     DNdir.Subtract(Ndir.Multiplied(myOffset * Dr / R3));
 231   }
 232
 233   Ndir.Multiply(myOffset / R);
 234   // P(u)
 235   theValue.ChangeCoord().Add(Ndir);
 236   // P'(u)
 237   theD1.Add(gp_Vec(DNdir));
 238 }
 239
 240 void GeomEvaluator_OffsetCurve::CalculateD2(      gp_Pnt& theValue,
 241                                                   gp_Vec& theD1,
 242                                                   gp_Vec& theD2,
 243                                             const gp_Vec& theD3,
 244                                             const Standard_Boolean theIsDirChange) const
 245 {
 246   // P(u) = p(u) + Offset * Ndir / R
 247   // with R = || p' ^ V|| and Ndir = P' ^ direction (local normal direction)
 248
 249   // P'(u) = p'(u) + (Offset / R**2) * (DNdir/DU * R -  Ndir * (DR/R))
 250
 251   // P"(u) = p"(u) + (Offset / R) * (D2Ndir/DU - DNdir * (2.0 * Dr/ R**2) +
 252   //         Ndir * ( (3.0 * Dr**2 / R**4) - (D2r / R**2)))
 253
 254   gp_XYZ Ndir = (theD1.XYZ()).Crossed(myOffsetDir.XYZ());
 255   gp_XYZ DNdir = (theD2.XYZ()).Crossed(myOffsetDir.XYZ());
 256   gp_XYZ D2Ndir = (theD3.XYZ()).Crossed(myOffsetDir.XYZ());
 257   Standard_Real R2 = Ndir.SquareModulus();
 258   Standard_Real R = Sqrt(R2);
 259   Standard_Real R3 = R2 * R;
 260   Standard_Real R4 = R2 * R2;
 261   Standard_Real R5 = R3 * R2;
 262   Standard_Real Dr = Ndir.Dot(DNdir);
 263   Standard_Real D2r = Ndir.Dot(D2Ndir) + DNdir.Dot(DNdir);
 264
 265   if (R5 <= gp::Resolution()) {
 266     if (R4 <= gp::Resolution())
 267       throw Standard_NullValue("GeomEvaluator_OffsetCurve: Null derivative");
 268     //We try another computation but the stability is not very good
 269     //dixit ISG.
 270     // V2 = P" (U) :
 271     R4 = R2 * R2;
 272     D2Ndir.Subtract(DNdir.Multiplied(2.0 * Dr / R2));
 273     D2Ndir.Add(Ndir.Multiplied(((3.0 * Dr * Dr) / R4) - (D2r / R2)));
 274     D2Ndir.Multiply(myOffset / R);
 275
 276     // V1 = P' (U) :
 277     DNdir.Multiply(R);
 278     DNdir.Subtract(Ndir.Multiplied(Dr / R));
 279     DNdir.Multiply(myOffset / R2);
 280   }
 281   else {
 282     // Same computation as IICURV in EUCLID-IS because the stability is better.
 283     // V2 = P" (U) :
 284     D2Ndir.Multiply(myOffset / R);
 285     D2Ndir.Subtract(DNdir.Multiplied(2.0 * myOffset * Dr / R3));
 286     D2Ndir.Add(Ndir.Multiplied(myOffset * (((3.0 * Dr * Dr) / R5) - (D2r / R3))));
 287
 288     // V1 = P' (U) :
 289     DNdir.Multiply(myOffset / R);
 290     DNdir.Subtract(Ndir.Multiplied(myOffset * Dr / R3));
 291   }
 292
 293   Ndir.Multiply(myOffset / R);
 294   // P(u)
 295   theValue.ChangeCoord().Add(Ndir);
 296   // P'(u) :
 297   theD1.Add(gp_Vec(DNdir));
 298   // P"(u) :
 299   if (theIsDirChange)
 300     theD2.Reverse();
 301   theD2.Add(gp_Vec(D2Ndir));
 302 }
 303
 304 void GeomEvaluator_OffsetCurve::CalculateD3(      gp_Pnt& theValue,
 305                                                   gp_Vec& theD1,
 306                                                   gp_Vec& theD2,
 307                                                   gp_Vec& theD3,
 308                                             const gp_Vec& theD4,
 309                                             const Standard_Boolean theIsDirChange) const
 310 {
 311   // P(u) = p(u) + Offset * Ndir / R
 312   // with R = || p' ^ V|| and Ndir = P' ^ direction (local normal direction)
 313
 314   // P'(u) = p'(u) + (Offset / R**2) * (DNdir/DU * R -  Ndir * (DR/R))
 315
 316   // P"(u) = p"(u) + (Offset / R) * (D2Ndir/DU - DNdir * (2.0 * Dr/ R**2) +
 317   //         Ndir * ( (3.0 * Dr**2 / R**4) - (D2r / R**2)))
 318
 319   //P"'(u) = p"'(u) + (Offset / R) * (D3Ndir - (3.0 * Dr/R**2) * D2Ndir -
 320   //         (3.0 * D2r / R2) * DNdir + (3.0 * Dr * Dr / R4) * DNdir -
 321   //         (D3r/R2) * Ndir + (6.0 * Dr * Dr / R4) * Ndir +
 322   //         (6.0 * Dr * D2r / R4) * Ndir - (15.0 * Dr* Dr* Dr /R6) * Ndir
 323
 324   gp_XYZ Ndir = (theD1.XYZ()).Crossed(myOffsetDir.XYZ());
 325   gp_XYZ DNdir = (theD2.XYZ()).Crossed(myOffsetDir.XYZ());
 326   gp_XYZ D2Ndir = (theD3.XYZ()).Crossed(myOffsetDir.XYZ());
 327   gp_XYZ D3Ndir = (theD4.XYZ()).Crossed(myOffsetDir.XYZ());
 328   Standard_Real R2 = Ndir.SquareModulus();
 329   Standard_Real R = Sqrt(R2);
 330   Standard_Real R3 = R2 * R;
 331   Standard_Real R4 = R2 * R2;
 332   Standard_Real R5 = R3 * R2;
 333   Standard_Real R6 = R3 * R3;
 334   Standard_Real R7 = R5 * R2;
 335   Standard_Real Dr = Ndir.Dot(DNdir);
 336   Standard_Real D2r = Ndir.Dot(D2Ndir) + DNdir.Dot(DNdir);
 337   Standard_Real D3r = Ndir.Dot(D3Ndir) + 3.0 * DNdir.Dot(D2Ndir);
 338   if (R7 <= gp::Resolution()) {
 339     if (R6 <= gp::Resolution())
 340       throw Standard_NullValue("CSLib_Offset: Null derivative");
 341     // V3 = P"' (U) :
 342     D3Ndir.Subtract(D2Ndir.Multiplied(3.0 * Dr / R2));
 343     D3Ndir.Subtract(DNdir.Multiplied(3.0 * ((D2r / R2) + (Dr*Dr / R4))));
 344     D3Ndir.Add(Ndir.Multiplied(6.0*Dr*Dr / R4 + 6.0*Dr*D2r / R4 - 15.0*Dr*Dr*Dr / R6 - D3r));
 345     D3Ndir.Multiply(myOffset / R);
 346     // V2 = P" (U) :
 347     R4 = R2 * R2;
 348     D2Ndir.Subtract(DNdir.Multiplied(2.0 * Dr / R2));
 349     D2Ndir.Subtract(Ndir.Multiplied((3.0 * Dr * Dr / R4) - (D2r / R2)));
 350     D2Ndir.Multiply(myOffset / R);
 351     // V1 = P' (U) :
 352     DNdir.Multiply(R);
 353     DNdir.Subtract(Ndir.Multiplied(Dr / R));
 354     DNdir.Multiply(myOffset / R2);
 355   }
 356   else {
 357     // V3 = P"' (U) :
 358     D3Ndir.Divide(R);
 359     D3Ndir.Subtract(D2Ndir.Multiplied(3.0 * Dr / R3));
 360     D3Ndir.Subtract(DNdir.Multiplied((3.0 * ((D2r / R3) + (Dr*Dr) / R5))));
 361     D3Ndir.Add(Ndir.Multiplied(6.0*Dr*Dr / R5 + 6.0*Dr*D2r / R5 - 15.0*Dr*Dr*Dr / R7 - D3r));
 362     D3Ndir.Multiply(myOffset);
 363     // V2 = P" (U) :
 364     D2Ndir.Divide(R);
 365     D2Ndir.Subtract(DNdir.Multiplied(2.0 * Dr / R3));
 366     D2Ndir.Subtract(Ndir.Multiplied((3.0 * Dr * Dr / R5) - (D2r / R3)));
 367     D2Ndir.Multiply(myOffset);
 368     // V1 = P' (U) :
 369     DNdir.Multiply(myOffset / R);
 370     DNdir.Subtract(Ndir.Multiplied(myOffset * Dr / R3));
 371   }
 372
 373   Ndir.Multiply(myOffset / R);
 374   // P(u)
 375   theValue.ChangeCoord().Add(Ndir);
 376   // P'(u) :
 377   theD1.Add(gp_Vec(DNdir));
 378   // P"(u)
 379   theD2.Add(gp_Vec(D2Ndir));
 380   // P"'(u)
 381   if (theIsDirChange)
 382     theD3.Reverse();
 383   theD3.Add(gp_Vec(D2Ndir));
 384 }
 385
 386
 387 Standard_Boolean GeomEvaluator_OffsetCurve::AdjustDerivative(
 388     const Standard_Integer theMaxDerivative, const Standard_Real theU,
 389     gp_Vec& theD1, gp_Vec& theD2, gp_Vec& theD3, gp_Vec& theD4) const
 390 {
 391   static const Standard_Real aTol = gp::Resolution();
 392   static const Standard_Real aMinStep = 1e-7;
 393   static const Standard_Integer aMaxDerivOrder = 3;
 394
 395   Standard_Boolean isDirectionChange = Standard_False;
 396   Standard_Real anUinfium;
 397   Standard_Real anUsupremum;
 398   if (!myBaseAdaptor.IsNull())
 399   {
 400     anUinfium = myBaseAdaptor->FirstParameter();
 401     anUsupremum = myBaseAdaptor->LastParameter();
 402   }
 403   else
 404   {
 405     anUinfium = myBaseCurve->FirstParameter();
 406     anUsupremum = myBaseCurve->LastParameter();
 407   }
 408
 409   static const Standard_Real DivisionFactor = 1.e-3;
 410   Standard_Real du;
 411   if ((anUsupremum >= RealLast()) || (anUinfium <= RealFirst()))
 412     du = 0.0;
 413   else
 414     du = anUsupremum - anUinfium;
 415
 416   const Standard_Real aDelta = Max(du * DivisionFactor, aMinStep);
 417
 418   //Derivative is approximated by Taylor-series
 419   Standard_Integer anIndex = 1; //Derivative order
 420   gp_Vec V;
 421
 422   do
 423   {
 424     V = BaseDN(theU, ++anIndex);
 425   } while ((V.SquareMagnitude() <= aTol) && anIndex < aMaxDerivOrder);
 426
 427   Standard_Real u;
 428
 429   if (theU - anUinfium < aDelta)
 430     u = theU + aDelta;
 431   else
 432     u = theU - aDelta;
 433
 434   gp_Pnt P1, P2;
 435   BaseD0(Min(theU, u), P1);
 436   BaseD0(Max(theU, u), P2);
 437
 438   gp_Vec V1(P1, P2);
 439   isDirectionChange = V.Dot(V1) < 0.0;
 440   Standard_Real aSign = isDirectionChange ? -1.0 : 1.0;
 441
 442   theD1 = V * aSign;
 443   gp_Vec* aDeriv[3] = { &theD2, &theD3, &theD4 };
 444   for (Standard_Integer i = 1; i < theMaxDerivative; i++)
 445     *(aDeriv[i - 1]) = BaseDN(theU, anIndex + i) * aSign;
 446
 447   return isDirectionChange;
 448 }
 449