Panda3D

nurbsCurveResult.cxx

00001 // Filename: nurbsCurveResult.cxx
00002 // Created by:  drose (04Dec02)
00003 //
00004 ////////////////////////////////////////////////////////////////////
00005 //
00006 // PANDA 3D SOFTWARE
00007 // Copyright (c) Carnegie Mellon University.  All rights reserved.
00008 //
00009 // All use of this software is subject to the terms of the revised BSD
00010 // license.  You should have received a copy of this license along
00011 // with this source code in a file named "LICENSE."
00012 //
00013 ////////////////////////////////////////////////////////////////////
00014 
00015 #include "nurbsCurveResult.h"
00016 #include "nurbsVertex.h"
00017 
00018 
00019 ////////////////////////////////////////////////////////////////////
00020 //     Function: NurbsCurveResult::Constructor
00021 //       Access: Public
00022 //  Description: The constructor automatically builds up the result as
00023 //               the product of the indicated set of basis matrices
00024 //               and the indicated table of control vertex positions.
00025 ////////////////////////////////////////////////////////////////////
00026 NurbsCurveResult::
00027 NurbsCurveResult(const NurbsBasisVector &basis,
00028                  const LVecBase4 vecs[], const NurbsVertex *verts,
00029                  int num_vertices) :
00030   _basis(basis),
00031   _verts(verts)
00032 {
00033   _last_segment = -1;
00034   int order = _basis.get_order();
00035   int num_segments = _basis.get_num_segments();
00036 
00037   _composed.reserve(num_segments);
00038   for (int i = 0; i < num_segments; i++) {
00039     int vi = _basis.get_vertex_index(i);
00040     nassertv(vi >= 0 && vi + order - 1 < num_vertices);
00041 
00042     // Create a geometry matrix from our (up to) four involved vertices.
00043     LMatrix4 geom;
00044     int ci = 0;
00045     while (ci < order) {
00046       geom.set_row(ci, vecs[vi + ci]);
00047       ci++;
00048     }
00049     while (ci < 4) {
00050       geom.set_row(ci, LVecBase4::zero());
00051       ci++;
00052     }
00053 
00054     // And compose this geometry matrix with the basis matrix to
00055     // produce a new matrix, which will be used to evaluate the curve.
00056     LMatrix4 result;
00057     result.multiply(_basis.get_basis(i), geom);
00058     _composed.push_back(result);
00059   }
00060 }
00061 
00062 ////////////////////////////////////////////////////////////////////
00063 //     Function: NurbsCurveResult::eval_segment_point
00064 //       Access: Published
00065 //  Description: Evaluates the point on the curve corresponding to the
00066 //               indicated value in parametric time within the
00067 //               indicated curve segment.  t should be in the range
00068 //               [0, 1].
00069 //
00070 //               The curve is internally represented as a number of
00071 //               connected (or possibly unconnected) piecewise
00072 //               continuous segments.  The exact number of segments
00073 //               for a particular curve depends on the knot vector,
00074 //               and is returned by get_num_segments().  Normally,
00075 //               eval_point() is used to evaluate a point along the
00076 //               continuous curve, but when you care more about local
00077 //               continuity, you can use eval_segment_point() to
00078 //               evaluate the points along each segment.
00079 ////////////////////////////////////////////////////////////////////
00080 void NurbsCurveResult::
00081 eval_segment_point(int segment, PN_stdfloat t, LVecBase3 &point) const {
00082   PN_stdfloat t2 = t*t;
00083   LVecBase4 tvec(t*t2, t2, t, 1.0f);
00084 
00085   PN_stdfloat weight = tvec.dot(_composed[segment].get_col(3));
00086 
00087   point.set(tvec.dot(_composed[segment].get_col(0)) / weight,
00088             tvec.dot(_composed[segment].get_col(1)) / weight,
00089             tvec.dot(_composed[segment].get_col(2)) / weight);
00090 }
00091 
00092 ////////////////////////////////////////////////////////////////////
00093 //     Function: NurbsCurveResult::eval_segment_tangent
00094 //       Access: Published
00095 //  Description: As eval_segment_point, but computes the tangent to
00096 //               the curve at the indicated point.  The tangent vector
00097 //               will not necessarily be normalized, and could be
00098 //               zero, particularly at the endpoints.
00099 ////////////////////////////////////////////////////////////////////
00100 void NurbsCurveResult::
00101 eval_segment_tangent(int segment, PN_stdfloat t, LVecBase3 &tangent) const {
00102   PN_stdfloat t2 = t*t;
00103   LVecBase4 tvec(3.0 * t2, 2.0 * t, 1.0f, 0.0f);
00104 
00105   tangent.set(tvec.dot(_composed[segment].get_col(0)),
00106               tvec.dot(_composed[segment].get_col(1)),
00107               tvec.dot(_composed[segment].get_col(2)));
00108 }
00109 
00110 ////////////////////////////////////////////////////////////////////
00111 //     Function: NurbsCurveResult::eval_segment_extended_point
00112 //       Access: Published
00113 //  Description: Evaluates the curve in n-dimensional space according
00114 //               to the extended vertices associated with the curve in
00115 //               the indicated dimension.
00116 ////////////////////////////////////////////////////////////////////
00117 PN_stdfloat NurbsCurveResult::
00118 eval_segment_extended_point(int segment, PN_stdfloat t, int d) const {
00119   nassertr(segment >= 0 && segment < _basis.get_num_segments(), 0.0f);
00120 
00121   PN_stdfloat t2 = t*t;
00122   LVecBase4 tvec(t*t2, t2, t, 1.0f);
00123 
00124   PN_stdfloat weight = tvec.dot(_composed[segment].get_col(3));
00125 
00126   // Calculate the composition of the basis matrix and the geometry
00127   // matrix on-the-fly.
00128   int order = _basis.get_order();
00129   int vi = _basis.get_vertex_index(segment);
00130 
00131   LVecBase4 geom;
00132   int ci = 0;
00133   while (ci < order) {
00134     geom[ci] = _verts[vi + ci].get_extended_vertex(d);
00135     ci++;
00136   }
00137   while (ci < 4) {
00138     geom[ci] = 0.0f;
00139     ci++;
00140   }
00141 
00142   const LMatrix4 &basis = _basis.get_basis(segment);
00143 
00144   // Compute matrix * column vector.
00145   LVecBase4 composed_geom(basis.get_row(0).dot(geom),
00146                            basis.get_row(1).dot(geom),
00147                            basis.get_row(2).dot(geom),
00148                            basis.get_row(3).dot(geom));
00149   return tvec.dot(composed_geom) / weight;
00150 }
00151 
00152 ////////////////////////////////////////////////////////////////////
00153 //     Function: NurbsCurveResult::eval_segment_extended_points
00154 //       Access: Published
00155 //  Description: Simultaneously performs eval_extended_point on a
00156 //               contiguous sequence of dimensions.  The dimensions
00157 //               evaluated are d through (d + num_values - 1); the
00158 //               results are filled into the num_values elements in
00159 //               the indicated result array.
00160 ////////////////////////////////////////////////////////////////////
00161 void NurbsCurveResult::
00162 eval_segment_extended_points(int segment, PN_stdfloat t, int d,
00163                              PN_stdfloat result[], int num_values) const {
00164   nassertv(segment >= 0 && segment < _basis.get_num_segments());
00165 
00166   PN_stdfloat t2 = t*t;
00167   LVecBase4 tvec(t*t2, t2, t, 1.0f);
00168 
00169   PN_stdfloat weight = tvec.dot(_composed[segment].get_col(3));
00170 
00171   // Calculate the composition of the basis matrix and the geometry
00172   // matrix on-the-fly.
00173   const LMatrix4 &basis = _basis.get_basis(segment);
00174   int order = _basis.get_order();
00175   int vi = _basis.get_vertex_index(segment);
00176 
00177   for (int n = 0; n < num_values; n++) {
00178     LVecBase4 geom;
00179     int ci = 0;
00180     while (ci < order) {
00181       geom[ci] = _verts[vi + ci].get_extended_vertex(d + n);
00182       ci++;
00183     }
00184     while (ci < 4) {
00185       geom[ci] = 0.0f;
00186       ci++;
00187     }
00188 
00189     // Compute matrix * column vector.
00190     LVecBase4 composed_geom(basis.get_row(0).dot(geom),
00191                              basis.get_row(1).dot(geom),
00192                              basis.get_row(2).dot(geom),
00193                              basis.get_row(3).dot(geom));
00194     result[n] = tvec.dot(composed_geom) / weight;
00195   }
00196 }
00197 
00198 ////////////////////////////////////////////////////////////////////
00199 //     Function: NurbsCurveResult::adaptive_sample
00200 //       Access: Published
00201 //  Description: Determines the set of subdivisions necessary to
00202 //               approximate the curve with a set of linear segments,
00203 //               no point of which is farther than tolerance units
00204 //               from the actual curve.
00205 //
00206 //               After this call, you may walk through the resulting
00207 //               set of samples with get_num_samples(),
00208 //               get_sample_t(), and get_sample_point().
00209 ////////////////////////////////////////////////////////////////////
00210 void NurbsCurveResult::
00211 adaptive_sample(PN_stdfloat tolerance) {
00212   PN_stdfloat tolerance_2 = tolerance * tolerance;
00213   _adaptive_result.clear();
00214 
00215   LPoint3 p0, p1;
00216 
00217   int num_segments = _basis.get_num_segments();
00218   for (int segment = 0; segment < num_segments; ++segment) {
00219     eval_segment_point(segment, 0.0f, p0);
00220     if (segment == 0 || !p0.almost_equal(p1)) {
00221       // We explicitly push the first point, and the boundary point
00222       // anytime the segment boundary is discontinuous.
00223       _adaptive_result.push_back(AdaptiveSample(_basis.get_from(segment), p0));
00224     }
00225 
00226     eval_segment_point(segment, 1.0f, p1);
00227 
00228     // Then we recusrively get the remaining points in the segment.
00229     r_adaptive_sample(segment, 0.0f, p0, 1.0f, p1, tolerance_2);
00230   }
00231 }
00232 
00233 ////////////////////////////////////////////////////////////////////
00234 //     Function: NurbsCurveResult::find_segment
00235 //       Access: Private
00236 //  Description: Returns the index of the segment that contains the
00237 //               indicated value of t, or -1 if no segment contains
00238 //               this value.
00239 ////////////////////////////////////////////////////////////////////
00240 int NurbsCurveResult::
00241 find_segment(PN_stdfloat t) {
00242   // Trivially check the endpoints of the curve.
00243   if (t >= get_end_t()) {
00244     return _basis.get_num_segments() - 1;
00245   } else if (t <= get_start_t()) {
00246     return 0;
00247   }
00248 
00249   // Check the last segment we searched for.  Often, two consecutive
00250   // requests are for the same segment.
00251   if (_last_segment != -1 && (t >= _last_from && t < _last_to)) {
00252     return _last_segment;
00253   }
00254 
00255   // Look for the segment the hard way.
00256   int segment = r_find_segment(t, 0, _basis.get_num_segments() - 1);
00257   if (segment != -1) {
00258     _last_segment = segment;
00259     _last_from = _basis.get_from(segment);
00260     _last_to = _basis.get_to(segment);
00261   }
00262   return segment;
00263 }
00264 
00265 ////////////////////////////////////////////////////////////////////
00266 //     Function: NurbsCurveResult::r_find_segment
00267 //       Access: Private
00268 //  Description: Recursively searches for the segment that contains
00269 //               the indicated value of t by performing a binary
00270 //               search.  This assumes the segments are stored in
00271 //               increasing order of t, and they don't overlap.
00272 ////////////////////////////////////////////////////////////////////
00273 int NurbsCurveResult::
00274 r_find_segment(PN_stdfloat t, int top, int bot) const {
00275   if (bot < top) {
00276     // Not found.
00277     return -1;
00278   }
00279   int mid = (top + bot) / 2;
00280   nassertr(mid >= 0 && mid < _basis.get_num_segments(), -1);
00281 
00282   PN_stdfloat from = _basis.get_from(mid);
00283   PN_stdfloat to = _basis.get_to(mid);
00284   if (from > t) {
00285     // Too high, try lower.
00286     return r_find_segment(t, top, mid - 1);
00287 
00288   } else if (to <= t) {
00289     // Too low, try higher.
00290     return r_find_segment(t, mid + 1, bot);
00291 
00292   } else {
00293     // Here we are!
00294     return mid;
00295   }
00296 }
00297 
00298 ////////////////////////////////////////////////////////////////////
00299 //     Function: NurbsCurveResult::r_adaptive_sample
00300 //       Access: Private
00301 //  Description: Recursively subdivides a potential evaluation of the
00302 //               segment until it is found to be within tolerance.
00303 //               This will add everything up to and including t1, but
00304 //               excluding t0.
00305 ////////////////////////////////////////////////////////////////////
00306 void NurbsCurveResult::
00307 r_adaptive_sample(int segment, PN_stdfloat t0, const LPoint3 &p0, 
00308                   PN_stdfloat t1, const LPoint3 &p1, PN_stdfloat tolerance_2) {
00309   PN_stdfloat tmid = (t0 + t1) * 0.5f;
00310   LPoint3 pmid;
00311   eval_segment_point(segment, tmid, pmid);
00312 
00313   if (sqr_dist_to_line(pmid, p0, p1 - p0) > tolerance_2) {
00314     // The line is still too curved--subdivide.
00315     r_adaptive_sample(segment, t0, p0, tmid, pmid, tolerance_2);
00316     r_adaptive_sample(segment, tmid, pmid, t1, p1, tolerance_2);
00317 
00318   } else {
00319     // The line is sufficiently close.  Stop here.
00320     _adaptive_result.push_back(AdaptiveSample(_basis.scale_t(segment, t1), p1));
00321   }
00322 }
00323 
00324 ////////////////////////////////////////////////////////////////////
00325 //     Function: NurbsCurveResult::sqr_dist_to_line
00326 //       Access: Private, Static
00327 //  Description: A support function for r_adaptive_sample(), this
00328 //               computes the minimum distance from a point to a line,
00329 //               and returns the distance squared.
00330 ////////////////////////////////////////////////////////////////////
00331 PN_stdfloat NurbsCurveResult::
00332 sqr_dist_to_line(const LPoint3 &point, const LPoint3 &origin, 
00333                  const LVector3 &vec) {
00334   LVector3 norm = vec;
00335   norm.normalize();
00336   LVector3 d = point - origin;
00337   PN_stdfloat hyp_2 = d.length_squared();
00338   PN_stdfloat leg = d.dot(norm);
00339   return hyp_2 - leg * leg;
00340 }
 All Classes Functions Variables Enumerations