MercuryDPM  Trunk
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
NurbsSurface.cc
Go to the documentation of this file.
1 //Copyright (c) 2013-2020, The MercuryDPM Developers Team. All rights reserved.
2 //For the list of developers, see <http://www.MercuryDPM.org/Team>.
3 //
4 //Redistribution and use in source and binary forms, with or without
5 //modification, are permitted provided that the following conditions are met:
6 // * Redistributions of source code must retain the above copyright
7 // notice, this list of conditions and the following disclaimer.
8 // * Redistributions in binary form must reproduce the above copyright
9 // notice, this list of conditions and the following disclaimer in the
10 // documentation and/or other materials provided with the distribution.
11 // * Neither the name MercuryDPM nor the
12 // names of its contributors may be used to endorse or promote products
13 // derived from this software without specific prior written permission.
14 //
15 //THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
16 //ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
17 //WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
18 //DISCLAIMED. IN NO EVENT SHALL THE MERCURYDPM DEVELOPERS TEAM BE LIABLE FOR ANY
19 //DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
20 //(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
21 //LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
22 //ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
23 //(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
24 //SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25 
26 #include <array>
27 #include "NurbsSurface.h"
28 #include "NurbsUtils.h"
29 #include "Logger.h"
30 #include "Math/ExtendedMath.h"
31 
32 using namespace NurbsUtils;
33 
35  std::vector<double> knotsU = {0,0,1,1};
36  std::vector<double> knotsV = {0,0,1,1};
37  std::vector<std::vector<Vec3D>> controlPoints = {{{0,0,0},{0,1,0}},{{1,0,0},{1,1,0}}} ;
38  std::vector<std::vector<Mdouble>> weights = {{1,1},{1,1}};
39  set(knotsU,knotsV,controlPoints,weights);
40  //todo think of good default
41 }
42 
43 NurbsSurface::NurbsSurface(const std::vector<double>& knotsU, const std::vector<double>& knotsV,
44  const std::vector<std::vector<Vec3D>>& controlPoints, const std::vector<std::vector<double>>& weights)
45 {
46  set(knotsU,knotsV,controlPoints,weights);
47  logger(INFO,"Created Nurbs surface.");
48  logger(INFO," %x% knots",knotsU.size(), knotsV.size());
49  logger(INFO," %x% control points",controlPoints.size(), controlPoints[0].size());
50  logger(INFO," %x% degrees",degreeU_,degreeV_);
51 }
52 
53 void NurbsSurface::set(const std::vector<double>& knotsU, const std::vector<double>& knotsV,
54  const std::vector<std::vector<Vec3D>>& controlPoints, const std::vector<std::vector<double>>& weights)
55 {
56  controlPoints_ = controlPoints;
57  weights_ = weights;
58  knotsU_ = knotsU;
59  knotsV_ = knotsV;
60 
61  if ( controlPoints.size()<2 || controlPoints[0].size()<2 ) {
62  logger(ERROR,"At least two control points are necessary");
63  }
64 
65  if (std::any_of(controlPoints.begin(), controlPoints.end(),
66  [controlPoints](auto v) { return v.size() != controlPoints[0].size(); }))
67  {
68  logger(ERROR, "All rows of the control matrix must have the same size");
69  }
70 
71  if (controlPoints.size() != weights.size() ||
72  std::any_of(weights.begin(), weights.end(),
73  [controlPoints](auto v) { return v.size() != controlPoints[0].size(); }))
74  {
75  logger(ERROR, "Number of control points and weights must match");
76  }
77 
78  if (knotsU.size() < 2 || knotsV.size() < 2)
79  {
80  logger(ERROR, "At least two knots are necessary");
81  }
82 
83  if (!isKnotVectorMonotonic(knotsU) || !isKnotVectorMonotonic(knotsV))
84  {
85  logger(ERROR, "Knot vector(s) is not monotonic");
86  }
87 
88  if (knotsU.size() - controlPoints.size() < 2 || knotsV.size() - controlPoints[0].size() - 1 < 1)
89  {
90  logger(ERROR, "Degree has to be at least 1");
91  }
92 
93  //Reset the u/v interval to [0,1]
94  const int minKU = knotsU_.front();
95  const int maxKU = knotsU_.back();
96  for (auto& k : knotsU_) {
97  k = (k - minKU) / (maxKU - minKU);
98  }
99  const int minKV = knotsV_.front();
100  const int maxKV = knotsV_.back();
101  for (auto& k : knotsV_) {
102  k = (k - minKV) / (maxKV - minKV);
103  }
104 
105  degreeU_ = knotsU.size() - controlPoints.size() - 1;
106  degreeV_ = knotsV.size() - controlPoints[0].size() - 1;
107 
108  logger(INFO,"Created Nurbs surface.");
109  logger(INFO," %x% knots",knotsU.size(), knotsV.size());
110  logger(INFO," %x% control points",controlPoints.size(), controlPoints[0].size());
111  logger(INFO," %x% degrees",degreeU_,degreeV_);
112 
113  //\todo TW
114  closedInU_ = false;
115  closedInV_ = false;
116 }
117 
118 void NurbsSurface::setClosedInU(bool closedInU) {
119  closedInU_ = closedInU;
120 }
121 
122 void NurbsSurface::setClosedInV(bool closedInV) {
123  closedInV_ = closedInV;
124 }
125 
127 {
128  for (auto& cp : controlPoints_) {
129  std::reverse(cp.begin(), cp.end());
130  }
131  for (auto& w : weights_) {
132  std::reverse(w.begin(), w.end());
133  }
134  std::reverse(knotsV_.begin(), knotsV_.end());
135  double maxK = knotsV_[0];
136  for (auto& k : knotsV_) {
137  k = maxK-k;
138  }
139  // \todo Fix logger messages
140  //logger(INFO,"Created Nurbs surface.");
141  //logger(INFO," %x% knots",knotsU.size(), knotsV.size());
142  //logger(INFO," %x% control points",controlPoints.size(), controlPoints[0].size());
143  //logger(INFO," %x% degrees",degreeU_,degreeV_);
144 }
145 
146 Vec3D NurbsSurface::evaluate(double u, double v) const {
147 
148  Vec3D point = {0,0,0};
149  double temp = 0;
150 
151  // Find span and non-zero basis functions
152  int spanU = findSpan(degreeU_, knotsU_, u);
153  int spanV = findSpan(degreeV_, knotsV_, v);
154  std::vector<double> Nu, Nv;
155  bsplineBasis(degreeU_, spanU, knotsU_, u, Nu);
156  bsplineBasis(degreeV_, spanV, knotsV_, v, Nv);
157  // make linear combination
158  for (int l = 0; l <= degreeV_; ++l)
159  {
160  for (int k = 0; k <= degreeU_; ++k)
161  {
162  double weight = Nv[l] * Nu[k] * weights_[spanU - degreeU_ + k][spanV - degreeV_ + l];
163  point += weight * controlPoints_[spanU - degreeU_ + k][spanV - degreeV_ + l];
164  temp += weight;
165  }
166  }
167  point /= temp;
168  return point;
169 }
170 
174 bool NurbsSurface::getDistance(Vec3D P, double radius, double& distance, Vec3D& normal) const {
175  // find the closest control point
176  double u;
177  double v;
178  double minDist2 = constants::inf;
179  for (int i=0; i<controlPoints_.size(); ++i) {
180  for (int j=0; j<controlPoints_[i].size(); ++j) {
181  const double dist2 = Vec3D::getLengthSquared(controlPoints_[i][j] - P);
182  if (dist2<minDist2) {
183  u = knotsU_[i];
184  v = knotsV_[j];
185  }
186  }
187  }
189  //Now do Newton-Raphson: Find closest point C(u) to P, see (6.6) in Nurbs book
190  // define f(u)=C'(u).(C(u)-P), find f(u)=0
191  // iterate u <- u-f(u)/f'(u)
192  //in 2D:
193  // define r(u,v) = S(u,v)-P
194  // f = r. dSdu, g = r. dSdv
195  // iterate u <- u + du, v <- v + dv
196  // J.[du;dv] = k
197  // J=[fu fv;gu gv], k=-[f;g]
198  std::array<std::array<Vec3D,3>,3> S;
199  const double tol = 2*std::numeric_limits<double>::epsilon();
200  const double tolSquared = mathsFunc::square<double>(tol);
201  Vec3D r;
202  double r1;
203  for (int i=0;i<15; ++i) {
204  //TW this algorithm does not compute contacts with the boundary
205  if (u<0) {
206  u = closedInU_?(u-static_cast<int>(u)):0;
207  } else if (u>1) {
208  u = closedInU_?(u-static_cast<int>(u)):1;
209  }
210  if (v<0) {
211  v = closedInV_?(v-static_cast<int>(u)):0;
212  } else if (v>1) {
213  v = closedInV_?(v-static_cast<int>(v)):1;
214  }
215  evaluateDerivatives(u,v,S);
216  r = S[0][0] - P;
217  r1 = r.getLengthSquared();
218  if (r1<tolSquared) /*first convergence criterium: contact point on surface*/ {
219  distance = 0;
220  normal = r;
221  return true;
222  }
223  const double r2 = mathsFunc::square<double>(Vec3D::dot(r, S[1][0]))/(r1*S[1][0].getLengthSquared());
224  const double r3 = mathsFunc::square<double>(Vec3D::dot(r, S[0][1]))/(r1*S[0][1].getLengthSquared());
225  if (std::fabs(r2)<tolSquared && std::fabs(r3)<tolSquared) /*second convergence criterium: zero cosine*/ {
226  //you should exit the function here
227  const bool inWall = Vec3D::dot(r,Vec3D::cross(S[1][0],S[0][1]))>=0;
228  if (inWall || r1<radius*radius) {
229  logger(VERBOSE,"contact found at % after % iterations",S[0][0],i);
230  distance = inWall ? -sqrt(r1) : sqrt(r1);
231  normal = r / distance;
232  return true;
233  } else {
234  logger(VERBOSE,"contact found, but too distant");
235  return false;
236  }
237  }
238  const double f = Vec3D::dot(r, S[1][0]);
239  const double g = Vec3D::dot(r, S[0][1]);
240  const double a = S[1][0].getLengthSquared() + Vec3D::dot(r, S[2][0]);
241  const double b = Vec3D::dot(S[1][0], S[0][1]) + Vec3D::dot(r, S[1][1]);
242  const double d = S[0][1].getLengthSquared() + Vec3D::dot(r, S[0][2]);
243  const double det = a * d - b * b;
244  const double du = (b * g - d * f) / det;
245  const double dv = (b * f - a * g) / det;
246  const double r4 = du*du*S[0][1].getLengthSquared()+dv*dv*S[1][0].getLengthSquared();
247  if (r4<tolSquared) /*third convergence criterium: parameters fixed*/ {
248  const bool inWall = Vec3D::dot(r,Vec3D::cross(S[1][0],S[0][1]))>=0;
249  if (inWall || r1<radius*radius) {
250  logger(VERBOSE,"parameters fixed, contact at % after % iterations",S[0][0],i);
251  distance = inWall ? -sqrt(r1) : sqrt(r1);
252  normal = r / distance;
253  return true;
254  } else {
255  logger(VERBOSE,"parameters fixed, but too distant");
256  return false;
257  }
258  }
259  u += du;
260  v += dv;
261  }
262  /* iteration fails */
263  logger(WARN,"P=%: Number of allowed iterations exceeded; this should never be reached",P);
264  const bool inWall = Vec3D::dot(r,Vec3D::cross(S[1][0],S[0][1]))>=0;
265  if (inWall || r1<radius*radius) {
266  logger(VERBOSE,"contact found at %",S[0][0]);
267  distance = inWall ? -sqrt(r1) : sqrt(r1);
268  normal = r / distance;
269  return true;
270  } else {
271  logger(VERBOSE,"contact found, but too distant");
272  return false;
273  }
274 }
275 
276 
287 void NurbsSurface::evaluateDerivatives(double u, double v, std::array<std::array<Vec3D,3>,3>& S) const
288 {
289  std::array<std::array<Vec3D,3>,3> A;
290  std::array<std::array<double,3>,3> w;
291  // Find span and non-zero basis functions
292  int spanU = findSpan(degreeU_, knotsU_, u);
293  int spanV = findSpan(degreeV_, knotsV_, v);
294  std::vector<std::vector<double>> Nu, Nv;
295  bsplineDerBasis(degreeU_, spanU, knotsU_, u, 2, Nu);
296  bsplineDerBasis(degreeV_, spanV, knotsV_, v, 2, Nv);
297  // make linear combination (eq 4.21 in Nurbs book)
298  for (int i=0; i<3; ++i) {
299  for (int j = 0; j < 3; ++j) {
300  A[i][j].setZero();
301  w[i][j] = 0;
302  for (int k = 0; k <= degreeU_; ++k) {
303  for (int l = 0; l <= degreeV_; ++l) {
304  double weight = Nu[i][k] * Nv[j][l] * weights_[spanU - degreeU_ + k][spanV - degreeV_ + l];
305  A[i][j] += weight * controlPoints_[spanU - degreeU_ + k][spanV - degreeV_ + l];
306  w[i][j] += weight;
307  }
308  }
309  }
310  }
311  S[0][0] = A[0][0]/w[0][0];
312  S[1][0] = (A[1][0]-w[1][0]*S[0][0])/w[0][0];
313  S[0][1] = (A[0][1]-w[0][1]*S[0][0])/w[0][0];
314  S[1][1] = (A[1][1]-w[1][1]*S[0][0]-w[1][0]*S[0][1]-w[0][1]*S[1][0])/w[0][0];
315  S[2][0] = (A[2][0]-2*w[1][0]*S[1][0]-w[2][0]*S[0][0])/w[0][0];
316  S[0][2] = (A[0][2]-2*w[0][1]*S[0][1]-w[0][2]*S[0][0])/w[0][0];
317 // std::cout << S[0][0] << std::endl;
318 // std::cout << S[1][0] << std::endl;
319 // std::cout << S[0][1] << std::endl;
320 // std::cout << S[2][0] << std::endl;
321 // std::cout << S[1][1] << std::endl;
322 // std::cout << S[0][2] << std::endl;
323 }
324 
325 
333 std::ostream& operator<<(std::ostream& os, const NurbsSurface& a)
334 {
335  os << "mu " << a.knotsU_.size() << ' ';
336  os << "mv " << a.knotsV_.size() << ' ';
337  os << "nu " << a.controlPoints_.size() << ' ';
338  os << "nv " << a.controlPoints_[0].size() << ' ';
339  os << "knotsU ";
340  for (const auto k : a.knotsU_) os << k << ' ';
341  os << "knotsV ";
342  for (const auto k : a.knotsV_) os << k << ' ';
343  os << "controlPoints ";
344  for (const auto cp0 : a.controlPoints_) for (const auto cp : cp0) os << cp << ' ';
345  os << "weights ";
346  for (const auto w0 : a.weights_) for (const auto w : w0) os << w << ' ';
347  return os;
348 }
349 
357 std::istream& operator>>(std::istream& is, NurbsSurface& a)
358 {
359  std::string dummy;
360  unsigned mu, mv, nu, nv;
361  is >> dummy >> mu;
362  is >> dummy >> mv;
363  is >> dummy >> nu;
364  is >> dummy >> nv;
365 
366  is >> dummy;
367  std::vector<double> knotsU;
368  knotsU.resize(mu);
369  for (auto& k : knotsU) is >> k;
370 
371  is >> dummy;
372  std::vector<double> knotsV;
373  knotsV.resize(mv);
374  for (auto& k : knotsV) is >> k;
375 
376  is >> dummy;
377  std::vector<std::vector<Vec3D>> controlPoints;
378  controlPoints.resize(nu);
379  for (auto& cp0 : controlPoints) {
380  cp0.resize(nv);
381  for (auto& cp : cp0) is >> cp;
382  }
383 
384  is >> dummy;
385  std::vector<std::vector<double>> weights;
386  weights.resize(nu);
387  for (auto& w0 : weights) {
388  w0.resize(nv);
389  for (auto& w : w0) is >> w;
390  }
391 
392  a.set(knotsU,knotsV,controlPoints,weights);
393  return is;
394 }
bool getDistance(Vec3D P, double radius, double &distance, Vec3D &normal) const
Find projection onto surface, return distance (and contactPoint)
void setClosedInV(bool closedInV)
std::vector< std::vector< Vec3D > > controlPoints_
nu x nv control points
Definition: NurbsSurface.h:113
void flipOrientation()
int findSpan(int degree, const std::vector< double > &knots, double u)
Find the span of the given parameter in the knot vector.
Definition: NurbsUtils.cc:43
Logger< MERCURY_LOGLEVEL > logger("MercuryKernel")
void set(const std::vector< double > &knotsU, const std::vector< double > &knotsV, const std::vector< std::vector< Vec3D >> &controlPoints, const std::vector< std::vector< double >> &weights)
Create a non-rational NurbsSurface.
Definition: NurbsSurface.cc:53
LL< Log::INFO > INFO
Info log level.
Definition: Logger.cc:55
std::vector< std::vector< double > > weights_
nu x nv weights
Definition: NurbsSurface.h:115
const std::complex< Mdouble > i
Definition: ExtendedMath.h:50
std::vector< double > knotsU_
mu knots
Definition: NurbsSurface.h:109
void setClosedInU(bool closedInU)
static Mdouble dot(const Vec3D &a, const Vec3D &b)
Calculates the dot product of two Vec3D: .
Definition: Vector.cc:76
LL< Log::ERROR > ERROR
Error log level.
Definition: Logger.cc:53
Mdouble getLengthSquared() const
Calculates the squared length of this Vec3D: .
Definition: Vector.cc:184
const Mdouble inf
Definition: GeneralDefine.h:44
NurbsSurface()
Create a non-rational NurbsSurface.
Definition: NurbsSurface.cc:34
std::ostream & operator<<(std::ostream &os, const NurbsSurface &a)
LL< Log::WARN > WARN
Warning log level.
Definition: Logger.cc:54
static Vec3D cross(const Vec3D &a, const Vec3D &b)
Calculates the cross product of two Vec3D: .
Definition: Vector.cc:163
LL< Log::VERBOSE > VERBOSE
Verbose information.
Definition: Logger.cc:57
std::istream & operator>>(std::istream &is, NurbsSurface &a)
std::vector< double > knotsV_
mv knots
Definition: NurbsSurface.h:111
Vec3D evaluate(double u, double v) const
Evaluate point on a nonrational NURBS surface.
static Mdouble getLengthSquared(const Vec3D &a)
Calculates the squared length of a Vec3D: .
Definition: Vector.h:316
bool isKnotVectorMonotonic(const std::vector< double > &knots)
Definition: NurbsUtils.cc:33
Definition: Vector.h:49
void bsplineDerBasis(int deg, int span, const std::vector< double > &knots, double u, int nDers, std::vector< std::vector< double >> &ders)
Compute all non-zero derivatives of B-spline basis functions.
Definition: NurbsUtils.cc:155
void evaluateDerivatives(double u, double v, std::array< std::array< Vec3D, 3 >, 3 > &S) const
Evaluate derivatives of a rational NURBS curve.
void bsplineBasis(int deg, int span, const std::vector< double > &knots, double u, std::vector< double > &N)
Compute all non-zero B-spline basis functions.
Definition: NurbsUtils.cc:129