00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015 #include "piecewiseCurve.h"
00016
00017 #include "config_parametrics.h"
00018 #include "hermiteCurve.h"
00019
00020 #include "datagram.h"
00021 #include "datagramIterator.h"
00022 #include "bamWriter.h"
00023 #include "bamReader.h"
00024
00025 TypeHandle CubicCurveseg::_type_handle;
00026
00027
00028
00029
00030
00031
00032 CubicCurveseg::
00033 CubicCurveseg() {
00034 }
00035
00036
00037
00038
00039
00040
00041
00042 CubicCurveseg::
00043 CubicCurveseg(const LMatrix4 &basis) {
00044 Bx = basis.get_col(0);
00045 By = basis.get_col(1);
00046 Bz = basis.get_col(2);
00047 Bw = basis.get_col(3);
00048 rational = true;
00049 }
00050
00051
00052
00053
00054
00055
00056
00057 CubicCurveseg::
00058 CubicCurveseg(const BezierSeg &seg) {
00059 bezier_basis(seg);
00060 }
00061
00062
00063
00064
00065
00066
00067
00068
00069 CubicCurveseg::
00070 CubicCurveseg(int order, const PN_stdfloat knots[], const LVecBase4 cvs[]) {
00071 nurbs_basis(order, knots, cvs);
00072 }
00073
00074
00075
00076
00077
00078
00079 CubicCurveseg::
00080 ~CubicCurveseg() {
00081 }
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091 bool CubicCurveseg::
00092 get_point(PN_stdfloat t, LVecBase3 &point) const {
00093 PN_stdfloat t_sqrd = t*t;
00094 evaluate_point(LVecBase4(t*t_sqrd, t_sqrd, t, 1.0f), point);
00095 return true;
00096 }
00097
00098
00099
00100
00101
00102
00103
00104 bool CubicCurveseg::
00105 get_tangent(PN_stdfloat t, LVecBase3 &tangent) const {
00106 evaluate_vector(LVecBase4(3.0f*t*t, 2.0f*t, 1.0f, 0.0f), tangent);
00107 return true;
00108 }
00109
00110
00111
00112
00113
00114
00115
00116 bool CubicCurveseg::
00117 get_pt(PN_stdfloat t, LVecBase3 &point, LVecBase3 &tangent) const {
00118 PN_stdfloat t_sqrd=t*t;
00119 evaluate_point(LVecBase4(t*t_sqrd, t_sqrd, t, 1.0f), point);
00120 evaluate_vector(LVecBase4(3.0f*t_sqrd, t+t, 1.0f, 0.0f), tangent);
00121 return true;
00122 }
00123
00124
00125
00126
00127
00128
00129
00130 bool CubicCurveseg::
00131 get_2ndtangent(PN_stdfloat t, LVecBase3 &tangent2) const {
00132 evaluate_vector(LVecBase4(6.0f*t, 2.0f, 0.0f, 0.0f), tangent2);
00133 return true;
00134 }
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144 void CubicCurveseg::
00145 hermite_basis(const HermiteCurveCV &cv0,
00146 const HermiteCurveCV &cv1,
00147 PN_stdfloat tlength) {
00148 static LMatrix4
00149 Mh( 2.0f, -3.0f, 0.0f, 1.0f,
00150 -2.0f, 3.0f, 0.0f, 0.0f,
00151 1.0f, -2.0f, 1.0f, 0.0f,
00152 1.0f, -1.0f, 0.0f, 0.0f);
00153
00154 LVecBase4 Gx(cv0._p[0], cv1._p[0],
00155 cv0._out[0]*tlength, cv1._in[0]*tlength);
00156 LVecBase4 Gy(cv0._p[1], cv1._p[1],
00157 cv0._out[1]*tlength, cv1._in[1]*tlength);
00158 LVecBase4 Gz(cv0._p[2], cv1._p[2],
00159 cv0._out[2]*tlength, cv1._in[2]*tlength);
00160
00161 Bx = Gx * Mh;
00162 By = Gy * Mh;
00163 Bz = Gz * Mh;
00164 rational = false;
00165 }
00166
00167
00168
00169
00170
00171
00172
00173
00174 void CubicCurveseg::
00175 bezier_basis(const BezierSeg &seg) {
00176 static LMatrix4
00177 Mb(-1.0f, 3.0f, -3.0f, 1.0f,
00178 3.0f, -6.0f, 3.0f, 0.0f,
00179 -3.0f, 3.0f, 0.0f, 0.0f,
00180 1.0f, 0.0f, 0.0f, 0.0f);
00181
00182 LVecBase4 Gx(seg._v[0][0], seg._v[1][0], seg._v[2][0], seg._v[3][0]);
00183 LVecBase4 Gy(seg._v[0][1], seg._v[1][1], seg._v[2][1], seg._v[3][1]);
00184 LVecBase4 Gz(seg._v[0][2], seg._v[1][2], seg._v[2][2], seg._v[3][2]);
00185
00186 Bx = Gx * Mb;
00187 By = Gy * Mb;
00188 Bz = Gz * Mb;
00189 rational = false;
00190 }
00191
00192 static LVecBase4
00193 nurbs_blending_function(int order, int i, int j,
00194 const PN_stdfloat knots[]) {
00195
00196 LVecBase4 r;
00197
00198 if (j==1) {
00199 if (i==order-1 && knots[i] < knots[i+1]) {
00200 r.set(0.0f, 0.0f, 0.0f, 1.0f);
00201 } else {
00202 r.set(0.0f, 0.0f, 0.0f, 0.0f);
00203 }
00204
00205 } else {
00206 LVecBase4 bi0 = nurbs_blending_function(order, i, j-1, knots);
00207 LVecBase4 bi1 = nurbs_blending_function(order, i+1, j-1, knots);
00208
00209 PN_stdfloat d0 = knots[i+j-1] - knots[i];
00210 PN_stdfloat d1 = knots[i+j] - knots[i+1];
00211
00212
00213 if (d0 != 0.0f) {
00214 if (d1 != 0.0f) {
00215 r = bi0 / d0 - bi1 / d1;
00216 } else {
00217 r = bi0 / d0;
00218 }
00219
00220 } else if (d1 != 0.0f) {
00221 r = - bi1 / d1;
00222
00223 } else {
00224 r.set(0.0f, 0.0f, 0.0f, 0.0f);
00225 }
00226
00227
00228 r[0] = r[1];
00229 r[1] = r[2];
00230 r[2] = r[3];
00231 r[3] = 0.0f;
00232
00233
00234 if (d0 != 0.0f) {
00235 if (d1 != 0.0f) {
00236 r += bi0 * (- knots[i] / d0) + bi1 * (knots[i+j] / d1);
00237 } else {
00238 r += bi0 * (- knots[i] / d0);
00239 }
00240
00241 } else if (d1 != 0.0f) {
00242 r += bi1 * (knots[i+j] / d1);
00243 }
00244 }
00245
00246 return r;
00247 }
00248
00249 void
00250 compute_nurbs_basis(int order,
00251 const PN_stdfloat knots_in[],
00252 LMatrix4 &basis) {
00253 int i;
00254
00255
00256 PN_stdfloat knots[8];
00257 PN_stdfloat mink = knots_in[order-1];
00258 PN_stdfloat maxk = knots_in[order];
00259
00260 if (mink==maxk) {
00261
00262 parametrics_cat->warning()
00263 << "Trivial NURBS curve specified." << endl;
00264 memset((void *)&basis, 0, sizeof(LMatrix4));
00265 return;
00266 }
00267
00268 for (i = 0; i<2*order; i++) {
00269 knots[i] = (knots_in[i] - mink) / (maxk-mink);
00270 }
00271
00272
00273 LVecBase4 b[4];
00274 for (i = 0; i<order; i++) {
00275 b[i] = nurbs_blending_function(order, i, order, knots);
00276 }
00277
00278 for (i = 0; i<order; i++) {
00279 basis.set_row(i, b[i]);
00280 }
00281
00282 for (i=order; i<4; i++) {
00283 basis.set_row(i, LVecBase4::zero());
00284 }
00285 }
00286
00287
00288
00289
00290
00291
00292
00293
00294
00295
00296
00297 void CubicCurveseg::
00298 nurbs_basis(int order, const PN_stdfloat knots[], const LVecBase4 cvs[]) {
00299 assert(order>=1 && order<=4);
00300
00301 LMatrix4 B;
00302 compute_nurbs_basis(order, knots, B);
00303
00304
00305
00306 LVecBase4 c[4];
00307 for (int i = 0; i < 4; i++) {
00308 c[i] = (i<order) ? cvs[i] : LVecBase4(0.0f, 0.0f, 0.0f, 0.0f);
00309 }
00310
00311 Bx = LVecBase4(c[0][0], c[1][0], c[2][0], c[3][0]) * B;
00312 By = LVecBase4(c[0][1], c[1][1], c[2][1], c[3][1]) * B;
00313 Bz = LVecBase4(c[0][2], c[1][2], c[2][2], c[3][2]) * B;
00314 Bw = LVecBase4(c[0][3], c[1][3], c[2][3], c[3][3]) * B;
00315
00316 rational = true;
00317 }
00318
00319
00320
00321
00322
00323
00324
00325
00326
00327 bool CubicCurveseg::
00328 get_bezier_seg(BezierSeg &seg) const {
00329 static LMatrix4
00330 Mbi(0.0f, 0.0f, 0.0f, 1.0f,
00331 0.0f, 0.0f, 1.0f/3.0f, 1.0f,
00332 0.0f, 1.0f/3.0f, 2.0f/3.0f, 1.0f,
00333 1.0f, 1.0f, 1.0f, 1.0f);
00334
00335 LVecBase4 Gx = Bx * Mbi;
00336 LVecBase4 Gy = By * Mbi;
00337 LVecBase4 Gz = Bz * Mbi;
00338
00339 if (rational) {
00340 LVecBase4 Gw = Bw * Mbi;
00341 seg._v[0].set(Gx[0]/Gw[0], Gy[0]/Gw[0], Gz[0]/Gw[0]);
00342 seg._v[1].set(Gx[1]/Gw[1], Gy[1]/Gw[1], Gz[1]/Gw[1]);
00343 seg._v[2].set(Gx[2]/Gw[2], Gy[2]/Gw[2], Gz[2]/Gw[2]);
00344 seg._v[3].set(Gx[3]/Gw[3], Gy[3]/Gw[3], Gz[3]/Gw[3]);
00345 } else {
00346 seg._v[0].set(Gx[0], Gy[0], Gz[0]);
00347 seg._v[1].set(Gx[1], Gy[1], Gz[1]);
00348 seg._v[2].set(Gx[2], Gy[2], Gz[2]);
00349 seg._v[3].set(Gx[3], Gy[3], Gz[3]);
00350 }
00351
00352 return true;
00353 }
00354
00355
00356 inline LVecBase4
00357 col_mult(const LMatrix4 &M, const LVecBase4 &v) {
00358 return LVecBase4(M(0,0)*v[0] + M(0,1)*v[1] + M(0,2)*v[2] + M(0,3)*v[3],
00359 M(1,0)*v[0] + M(1,1)*v[1] + M(1,2)*v[2] + M(1,3)*v[3],
00360 M(2,0)*v[0] + M(2,1)*v[1] + M(2,2)*v[2] + M(2,3)*v[3],
00361 M(3,0)*v[0] + M(3,1)*v[1] + M(3,2)*v[2] + M(3,3)*v[3]);
00362 }
00363
00364
00365
00366
00367
00368
00369
00370 static bool
00371 compute_seg_col(int c,
00372 int rtype, PN_stdfloat t, const LVecBase4 &v,
00373 const LMatrix4 &B,
00374 const LMatrix4 &Bi,
00375 const LMatrix4 &G,
00376 const LMatrix4 &GB,
00377 LMatrix4 &T, LMatrix4 &P) {
00378 bool keep_orig = ((rtype & RT_KEEP_ORIG) != 0);
00379
00380 if (parametrics_cat.is_debug()) {
00381 parametrics_cat.debug()
00382 << "Computing col " << c << " type " << (rtype & RT_BASE_TYPE)
00383 << " at " << t << " keep_orig = " << keep_orig
00384 << " v = " << v << "\n";
00385 }
00386
00387 switch (rtype & RT_BASE_TYPE) {
00388
00389
00390 PN_stdfloat t_sqrd,t_cubed;
00391
00392 case RT_POINT:
00393 t_sqrd = t*t;
00394 t_cubed = t_sqrd*t;
00395 T.set_col(c, LVecBase4(t_cubed, t_sqrd, t, 1.0f));
00396 if (keep_orig) {
00397 LVecBase4 vec(t_cubed, t_sqrd, t, 1.0f);
00398 LVecBase4 ov = col_mult(GB, vec);
00399 if (parametrics_cat.is_debug()) {
00400 parametrics_cat.debug()
00401 << "orig point = " << ov << "\n";
00402 }
00403 P.set_col(c, ov);
00404 } else {
00405 P.set_col(c, v);
00406 }
00407 break;
00408
00409
00410
00411 case RT_TANGENT:
00412 t_sqrd = t*t;
00413 T.set_col(c, LVecBase4(3.0f*t_sqrd, t+t, 1.0f, 0.0f));
00414 if (keep_orig) {
00415 LVecBase4 vec(3.0f*t_sqrd, t+t, 1.0f, 0.0f);
00416 LVecBase4 ov = col_mult(GB, vec);
00417 if (parametrics_cat.is_debug()) {
00418 parametrics_cat.debug()
00419 << "Matrix is:\n";
00420 GB.write(parametrics_cat.debug(false), 2);
00421 parametrics_cat.debug(false)
00422 << "vector is " << vec << "\n"
00423 << "orig tangent = " << ov << "\n";
00424 }
00425 P.set_col(c, ov);
00426 } else {
00427 P.set_col(c, v);
00428 }
00429 break;
00430
00431
00432
00433 case RT_CV:
00434 T.set_col(c, Bi.get_col(c));
00435 if (keep_orig) {
00436 if (parametrics_cat.is_debug()) {
00437 parametrics_cat.debug()
00438 << "orig CV = " << G.get_col(c) << "\n";
00439 }
00440 P.set_col(c, G.get_col(c));
00441 } else {
00442 P.set_col(c, v);
00443 }
00444 break;
00445
00446 default:
00447 cerr << "Invalid rebuild type in compute_seg\n";
00448 return false;
00449 }
00450
00451 return true;
00452 }
00453
00454
00455
00456
00457
00458
00459
00460
00461
00462
00463
00464
00465
00466
00467
00468
00469
00470
00471
00472
00473
00474
00475
00476
00477
00478
00479
00480
00481
00482
00483
00484
00485
00486
00487
00488
00489
00490
00491
00492
00493
00494
00495
00496
00497
00498
00499
00500
00501
00502
00503
00504
00505 bool CubicCurveseg::
00506 compute_seg(int rtype0, PN_stdfloat t0, const LVecBase4 &v0,
00507 int rtype1, PN_stdfloat t1, const LVecBase4 &v1,
00508 int rtype2, PN_stdfloat t2, const LVecBase4 &v2,
00509 int rtype3, PN_stdfloat t3, const LVecBase4 &v3,
00510 const LMatrix4 &B,
00511 const LMatrix4 &Bi,
00512 LMatrix4 &G) {
00513
00514
00515
00516
00517
00518
00519
00520
00521
00522
00523
00524
00525
00526
00527
00528
00529
00530
00531
00532 LMatrix4 T, P, GB;
00533
00534
00535
00536 if ((rtype0 | rtype1 | rtype2 | rtype3) & RT_KEEP_ORIG) {
00537 GB = G * B;
00538 }
00539
00540 if (! (compute_seg_col(0, rtype0, t0, v0, B, Bi, G, GB, T, P) &&
00541 compute_seg_col(1, rtype1, t1, v1, B, Bi, G, GB, T, P) &&
00542 compute_seg_col(2, rtype2, t2, v2, B, Bi, G, GB, T, P) &&
00543 compute_seg_col(3, rtype3, t3, v3, B, Bi, G, GB, T, P))) {
00544 return false;
00545 }
00546
00547 LMatrix4 Ti;
00548 Ti = invert(T);
00549
00550
00551
00552
00553
00554
00555
00556
00557 G = P * Ti * Bi;
00558
00559 return true;
00560 }
00561
00562
00563
00564
00565
00566
00567
00568 void CubicCurveseg::
00569 register_with_read_factory() {
00570 BamReader::get_factory()->register_factory(get_class_type(), make_CubicCurveseg);
00571 }
00572
00573
00574
00575
00576
00577
00578 TypedWritable *CubicCurveseg::
00579 make_CubicCurveseg(const FactoryParams ¶ms) {
00580 CubicCurveseg *me = new CubicCurveseg;
00581 DatagramIterator scan;
00582 BamReader *manager;
00583
00584 parse_params(params, scan, manager);
00585 me->fillin(scan, manager);
00586 return me;
00587 }
00588
00589
00590
00591
00592
00593
00594
00595 void CubicCurveseg::
00596 write_datagram(BamWriter *manager, Datagram &me) {
00597 ParametricCurve::write_datagram(manager, me);
00598
00599 Bx.write_datagram(me);
00600 By.write_datagram(me);
00601 Bz.write_datagram(me);
00602 Bw.write_datagram(me);
00603 me.add_bool(rational);
00604 }
00605
00606
00607
00608
00609
00610
00611
00612
00613
00614 void CubicCurveseg::
00615 fillin(DatagramIterator &scan, BamReader *manager) {
00616 ParametricCurve::fillin(scan, manager);
00617
00618 Bx.read_datagram(scan);
00619 By.read_datagram(scan);
00620 Bz.read_datagram(scan);
00621 Bw.read_datagram(scan);
00622 rational = scan.get_bool();
00623 }