Panda3D
|
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 }