00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015 #include "nurbsCurveResult.h"
00016 #include "nurbsVertex.h"
00017
00018
00019
00020
00021
00022
00023
00024
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
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
00055
00056 LMatrix4 result;
00057 result.multiply(_basis.get_basis(i), geom);
00058 _composed.push_back(result);
00059 }
00060 }
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
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
00094
00095
00096
00097
00098
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
00112
00113
00114
00115
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
00127
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
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
00154
00155
00156
00157
00158
00159
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
00172
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
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
00200
00201
00202
00203
00204
00205
00206
00207
00208
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
00222
00223 _adaptive_result.push_back(AdaptiveSample(_basis.get_from(segment), p0));
00224 }
00225
00226 eval_segment_point(segment, 1.0f, p1);
00227
00228
00229 r_adaptive_sample(segment, 0.0f, p0, 1.0f, p1, tolerance_2);
00230 }
00231 }
00232
00233
00234
00235
00236
00237
00238
00239
00240 int NurbsCurveResult::
00241 find_segment(PN_stdfloat t) {
00242
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
00250
00251 if (_last_segment != -1 && (t >= _last_from && t < _last_to)) {
00252 return _last_segment;
00253 }
00254
00255
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
00267
00268
00269
00270
00271
00272
00273 int NurbsCurveResult::
00274 r_find_segment(PN_stdfloat t, int top, int bot) const {
00275 if (bot < top) {
00276
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
00286 return r_find_segment(t, top, mid - 1);
00287
00288 } else if (to <= t) {
00289
00290 return r_find_segment(t, mid + 1, bot);
00291
00292 } else {
00293
00294 return mid;
00295 }
00296 }
00297
00298
00299
00300
00301
00302
00303
00304
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
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
00320 _adaptive_result.push_back(AdaptiveSample(_basis.scale_t(segment, t1), p1));
00321 }
00322 }
00323
00324
00325
00326
00327
00328
00329
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 }