2
Fab@Home operates under the BSD Open Source License
4
Copyright (c) 2006, Hod Lipson and Evan Malone (evan.malone@cornell.edu) All rights reserved.
6
Redistribution and use in source and binary forms, with or without modification,
7
are permitted provided that the following conditions are met:
9
Redistributions of source code must retain the above copyright notice,
10
this list of conditions and the following disclaimer.
11
Redistributions in binary form must reproduce the above copyright notice,
12
this list of conditions and the following disclaimer in the documentation and/or
13
other materials provided with the distribution.
14
Neither the name of the Fab@Home Project nor the names of its contributors may be
15
used to endorse or promote products derived from this software without specific
16
prior written permission.
18
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
20
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
21
IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
22
INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
23
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
24
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
25
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
26
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
27
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30
// Utils.cpp: implementation of the CUtils class.
32
//////////////////////////////////////////////////////////////////////
38
static char THIS_FILE[]=__FILE__;
42
//////////////////////////////////////////////////////////////////////
43
// Construction/Destruction
44
//////////////////////////////////////////////////////////////////////
46
//--------------------------------------------------------------------
48
//--------------------------------------------------------------------
53
//--------------------------------------------------------------------
55
//--------------------------------------------------------------------
62
// Copyright 2001, softSurfer (www.softsurfer.com)
63
// This code may be freely used and modified for any purpose
64
// providing that this copyright notice is included with it.
65
// SoftSurfer makes no warranty for this code, and cannot be held
66
// liable for any real or imagined damage resulting from its use.
67
// Users of this code must verify correctness for their application.
70
// Assume that a class is already given for the object:
71
// Point with coordinates {float x, y;}
72
//===================================================================
75
// isLeft(): tests if a point is Left|On|Right of an infinite line.
76
// Input: three points P0, P1, and P2
77
// Return: >0 for P2 left of the line through P0 and P1
78
// =0 for P2 on the line
79
// <0 for P2 right of the line
80
// See: the January 2001 Algorithm on Area of Triangles
82
isLeft( CVec& P0, CVec& P1, CVec& P2 )
84
return (P1.x - P0.x)*(P2.y - P0.y) - (P2.x - P0.x)*(P1.y - P0.y);
86
//===================================================================
89
// chainHull_2D(): Andrew's monotone chain 2D convex hull algorithm
90
// Input: P[] = an array of 2D points
91
// presorted by increasing x- and y-coordinates
92
// n = the number of points in P[]
93
// Output: H[] = an array of the convex hull vertices (max is n)
94
// idx[] = an array of indices into P[] of the vertices in H[]
95
// Return: the number of points in H[] (and idx[])
96
int CUtils::ChainHull_2D( CVec* P, int n, CVec* H, int* idx )
98
// the output array H[] will be used as the stack
99
int bot=0, top=(-1); // indices for bottom and top of the stack
100
int i; // array scan index
102
// Get the indices of points with min x-coord and min|max y-coord
103
int minmin = 0, minmax;
104
double xmin = P[0].x;
106
if (P[i].x != xmin) break;
108
if (minmax == n-1) { // degenerate case: all x-coords == xmin
109
H[++top] = P[minmin];
111
if (P[minmax].y != P[minmin].y){ // a nontrivial segment
112
H[++top] = P[minmax];
115
H[++top] = P[minmin]; // add polygon endpoint
120
// Get the indices of points with max x-coord and min|max y-coord
121
int maxmin, maxmax = n-1;
122
double xmax = P[n-1].x;
123
for (i=n-2; i>=0; i--)
124
if (P[i].x != xmax) break;
127
// Compute the lower hull on the stack H
128
H[++top] = P[minmin]; // push minmin point onto stack
131
while (++i <= maxmin)
133
// the lower line joins P[minmin] with P[maxmin]
134
if (isLeft( P[minmin], P[maxmin], P[i]) >= 0 && i < maxmin)
135
continue; // ignore P[i] above or on the lower line
137
while (top > 0) // there are at least 2 points on the stack
139
// test if P[i] is left of the line at the stack top
140
if (isLeft( H[top-1], H[top], P[i]) > 0)
141
break; // P[i] is a new hull vertex
143
top--; // pop top point off stack
145
H[++top] = P[i]; // push P[i] onto stack
149
// Next, compute the upper hull on the stack H above the bottom hull
150
if (maxmax != maxmin){ // if distinct xmax points
151
H[++top] = P[maxmax]; // push maxmax point onto stack
154
bot = top; // the bottom point of the upper hull stack
156
while (--i >= minmax)
158
// the upper line joins P[maxmax] with P[minmax]
159
if (isLeft( P[maxmax], P[minmax], P[i]) >= 0 && i > minmax)
160
continue; // ignore P[i] below or on the upper line
162
while (top > bot) // at least 2 points on the upper stack
164
// test if P[i] is left of the line at the stack top
165
if (isLeft( H[top-1], H[top], P[i]) > 0)
166
break; // P[i] is a new hull vertex
168
top--; // pop top point off stack
170
H[++top] = P[i]; // push P[i] onto stack
173
if (minmax != minmin){
174
H[++top] = P[minmin]; // push joining endpoint onto stack
183
//--------------------------------------------------------------------
184
CString CUtils::NoExtention(CString str)
185
//--------------------------------------------------------------------
186
{ // remove extention part from a file name
188
int p = str.ReverseFind('.');
197
//--------------------------------------------------------------------
198
CString CUtils::RemovePath(CString str)
199
//--------------------------------------------------------------------
200
{ // remove path part from a file name
202
int p = str.ReverseFind('\\');
211
/*----------------------------------------------------------------------*/
212
int CUtils::Solve(long N, double a[SOLVESIZE][SOLVESIZE], double b[SOLVESIZE])
213
/*--------------------------------------------------------------------- */
214
{ // Solve matrix Eqs. aX=b. Input: N=Order of matrices a & b
215
// Output: Solved X in b. Diagnostics: returns 0=Error, 1=OK
220
#define SOLVEPS 1E-12
222
/* perform pivoting */
224
for (i = 0; i < N; i++) {
225
if (fabs(a[i][i]) <= SOLVEPS) {
226
for (j = 0; j < N; j++) {
227
if (fabs(a[j][i]) > SOLVEPS && fabs(a[i][j]) > SOLVEPS) {
228
for (k = 0; k < N; k++) { /* swap rows i,j */
233
c = b[j]; /* swap const vector rows i,j */
240
if (fabs(a[i][i]) <= SOLVEPS)
241
return 0; /* Cannot pivot */
244
/* solve [a]*[x]=[b] */
246
for (i = 0; i < N - 1; i++) {
247
for (j = i + 1; j < N; j++) {
248
if (fabs(a[i][i]) <= SOLVEPS)
249
return 0; /* Zero Pivot */
250
f = a[j][i] / a[i][i];
251
for (k = i; k < N; k++)
252
a[j][k] = a[j][k] - a[i][k] * f;
253
b[j] = b[j] - b[i] * f;
257
/* back substitute and find solution [x] in [b] */
259
for (j = N - 1; j >= 0; j--) {
260
if (fabs(a[i][i]) <= SOLVEPS)
261
return 0; /* Zero Pivot */
262
b[j] = b[j] / a[j][j];
264
for (i = 0; i < j; i++) {
265
b[i] = b[i] - a[i][j] * b[j];
274
//--------------------------------------------------------------------
275
double CUtils::DistPointLine(CVec p, CVec p1, CVec p2, CVec &c, double& d)
276
//--------------------------------------------------------------------
277
{// distance of point from line.
280
double l = ax.Normalize();
284
return (p-p1).Length();
289
double len = (p-c).Length();
290
ASSERT(fabs((p-c).Dot(p2-p1))<1E-3);
295
//--------------------------------------------------------------------
296
double CUtils::ParameterAlongLine(CVec p, CVec p1, CVec p2)
297
//--------------------------------------------------------------------
298
{// parameter [0-1] along line segment
300
double dx = p2.x-p1.x;
301
double dy = p2.y-p1.y;
302
double dz = p2.z-p1.z;
305
if (fabs(dx)>fabs(dy) && fabs(dx)>fabs(dz)) {
307
} else if (fabs(dy)>fabs(dx) && fabs(dy)>fabs(dz)) {
317
//---------------------------------------------------------------------------
318
bool CUtils::SkewIntersection2(CVec A, CVec B, CVec C, CVec D, CVec& AB, CVec& CD)
319
//---------------------------------------------------------------------------
320
{// Find nearest crossing of two lines AB and CD in space.
321
// Return false if parallel, true otherwise
323
// The near intersection of two skew lines (from http://www.physik.uni-regensburg.de/psi/idl-4.0.1/jhuapl.doc/vectors/v_skew.html)
324
// Sometimes a problem arises where two lines in space should intersect but due to small measurement errors they are skew. The following figure shows the near intersection of two such skew lines. There is a point on each line that is closest to the other line. The midpoint of the segment connecting these points gives a good estimate of the desired intersection point.
325
// The desired intersection point is labeled M and shown in red.
327
// Line 1 is defined by points A and B, with unit vector U1 pointing along the line. Line 2 is defined by points C and D, with unit vector U2 pointing along the line.
328
// A vector, W, perpendicular to both lines is given by W = U1 cross U2. If the lines are parallel then the length of W is 0 and there is no one point of closest approach.
329
// Assuming non-parallel lines, a unit vector, U3, is given by
330
// U3 = W/|W| = (U1 cross U2)/|U1 cross U2|. This unit vector may point from Line 1 to Line 2, or in the opposite direction, depending on the directions of U1 and U2.
331
// A displacement vector from Line 2 to Line 1 may be found as follows. Let the vector from point C to point A be V = A-C. The length of V along U3 is (V dot U3) and is > 0 if U3 is parallel to V, and < 0 if U3 is anti-parallel to V. This length is also the distance between the lines. Since the sign of V reverses if U3 reverses, the product is a vector that always points the same way, from Line 2 to Line 1. So the displacement vector, E, needed to shift Line 2 to intersect Line 1 is given by
332
// E = (V dot U3)U3 = ((A-C) dot U3)U3.
333
// Line 2 is shifted as follows:
336
// Point I' is found using the algorithm for the intersection of two co-planar lines.
337
// Then I = I' - E/2.
338
// The distance by which the two lines miss each other is |E|, which may be used to check for abnormally large errors.
342
// Unit vector U1 along Line 1:
343
// U1 = (B-A)/| B-A |
344
CVec u1 = (B-A).Normalized();
345
// Unit vector U2 along Line 2:
346
// U2 = (D-C)/| D-C |
347
CVec u2 = (D-C).Normalized();
348
// Unit vector U3 from Line 2 toward Line 1:
349
// U3 = (U1 cross U2)/| U1 cross U2 |
350
CVec u3 = (u1.Cross(u2)).Normalized();
351
// Check for parallel lines
352
if (u3.x < 1E-4 && u3.x > -1E-4 &&
353
u3.y < 1E-4 && u3.y > -1E-4 &&
354
u3.z < 1E-4 && u3.z > -1E-4) {
355
ParallelOverlap(A,B,C,D,AB,CD);
358
// Displacement vector from Line 2 to Line 1:
359
// E = ((A-C) dot U3) U3
360
CVec E = ((A-C).Dot(u3))*u3;
366
// Unit vector U4 perpendicular to Line 2':
369
// V_perp = V - (V dot U2) U2
370
CVec V_perp = V - (V.Dot(u2))*u2;
371
// U4 = V_perp/| V_perp |
372
CVec u4 = V_perp.Normalized();
373
// Similar triangle side lengths P and p:
374
// P = (A - C') dot U4
375
double P = (A-Ct).Dot(u4);
376
// p = (B - C') dot U4
377
double p = (B-Ct).Dot(u4);
378
// Point I' fraction of distance from point A to point B:
379
// Frac = H/(H+h) = P/(P+p) !!!!ERROR Should be P/(P-p) -- Hod
381
// Intersection point I':
382
// I' = A + U1 * Frac !!!!ERROR Should be (B-A)*Frac) -- Hod
383
CVec It = A + (B-A)*f;
384
// Intersection point I:
386
// Proximity point on AB is at I'
388
// Proximity point on CD is at I'-E
391
double d0 = (AB-CD).Length();
394
double d1 = DistPointLine(AB,C,D,cc,dd);
395
double d2 = DistPointLine(CD,A,B,cc,dd);
396
if (fabs(d0-d1)>1E-2) TRACE("Mismatch1!!! %.2f %.2f\n", d0, d1);
397
if (fabs(d0-d2)>1E-2) TRACE("Mismatch2!!! %.2f %.2f\n", d0, d2);
398
if (fabs(d2-d1)>1E-2) {
399
TRACE("Mismatch3!!! %.2f %.2f\n", d2, d1);
400
TRACE("A=%.3f %.3f %.3f\n", A.x, A.y, A.z);
401
TRACE("B=%.3f %.3f %.3f\n", B.x, B.y, B.z);
402
TRACE("C=%.3f %.3f %.3f\n", C.x, C.y, C.z);
403
TRACE("D=%.3f %.3f %.3f\n", D.x, D.y, D.z);
404
TRACE("AB=%.3f %.3f %.3f\n", AB.x, AB.y, AB.z);
405
d1 = DistPointLine(AB,C,D,cc,dd);
406
double d1a = (AB-cc).Length(); // should be d1
407
double d1b = (CD-cc).Length(); // should be zero
408
TRACE("cc=%.3f %.3f %.3f\n", cc.x, cc.y, cc.z);
414
//---------------------------------------------------------------------------
415
bool CUtils::SkewIntersection(CVec A, CVec B, CVec C, CVec D, CVec& AB, CVec& CD)
416
//---------------------------------------------------------------------------
417
{// Find nearest crossing of two lines AB and CD in space.
418
// Return false if parallel, true otherwise
421
CVec ax1 = (B-A).Normalized();
422
CVec ax2 = (D-C).Normalized();
423
if (fabs(fabs(ax1.Dot(ax2))-1)<1E-4) return false; // parallel
425
CVec n = ax1.Cross(ax2);
427
double d = (C-A).Dot(n);
430
CVec zax = ax2.Cross(n).Normalized();
431
double dA2 = (A2-C).Dot(zax);
432
double dB2 = (B2-C).Dot(zax);
433
double pA2 = dA2/(dA2-dB2);
434
CD = A2 + pA2*(B2-A2);
439
double d0 = (AB-CD).Length();
440
double d1 = DistPointLine(AB,C,D,cc,dd);
441
double d2 = DistPointLine(CD,A,B,cc,dd);
442
if (fabs(d0-d1)>1E-2) TRACE("Mismatch1!!! %.2f %.2f\n", d0, d1);
443
if (fabs(d0-d2)>1E-2) TRACE("Mismatch2!!! %.2f %.2f\n", d0, d2);
444
if (fabs(d2-d1)>1E-2) {
445
TRACE("Mismatch3!!! %.2f %.2f\n", d2, d1);
446
TRACE("A=%.3f %.3f %.3f\n", A.x, A.y, A.z);
447
TRACE("B=%.3f %.3f %.3f\n", B.x, B.y, B.z);
448
TRACE("C=%.3f %.3f %.3f\n", C.x, C.y, C.z);
449
TRACE("D=%.3f %.3f %.3f\n", D.x, D.y, D.z);
450
TRACE("AB=%.3f %.3f %.3f\n", AB.x, AB.y, AB.z);
451
d1 = DistPointLine(AB,C,D,cc,dd);
452
double d1a = (AB-cc).Length(); // should be d1
453
double d1b = (CD-cc).Length(); // should be zero
454
TRACE("cc=%.3f %.3f %.3f\n", cc.x, cc.y, cc.z);
461
/*----------------------------------------------------------------------*/
462
bool CUtils::IsIntersectingLL(double x1, double y1, double x2, double y2, double x3, double y3, double x4, double y4)
463
/*----------------------------------------------------------------------*/
464
{// determine if two finite lines cross
468
if (x1 >= x3 && x1 >= x4 && x2 >= x3 && x2 >= x4)
470
if (x1 <= x3 && x1 <= x4 && x2 <= x3 && x2 <= x4)
472
if (y1 >= y3 && y1 >= y4 && y2 >= y3 && y2 >= y4)
474
if (y1 <= y3 && y1 <= y4 && y2 <= y3 && y2 <= y4)
481
double c1 = x2*y1 - x1*y2;
487
double c2 = x4*y3 - x3*y4;
489
// check for alternating signes
491
if ((a1*x3+b1*y3+c1)*(a1*x4+b1*y4+c1) < 0 &&
492
(a2*x1+b2*y1+c2)*(a2*x2+b2*y2+c2) < 0) {
493
return true; // interecting
495
return false; // not intersecting
500
//---------------------------------------------------------------------------
501
bool CUtils::ParallelOverlap(CVec A, CVec B, CVec C, CVec D, CVec& AB, CVec& CD)
502
//---------------------------------------------------------------------------
503
{// Find center of overlap region between two parallel lines AB and CD in space.
506
// Unit vector U1 along Line 1:
507
// U1 = (B-A)/| B-A |
508
CVec u1 = (B-A).Normalized();
509
// Displacement vector from Line 2 to Line 1:
510
// E = (A-C) - (A-C).dot(U1)*U1
511
CVec E = (A-C) - (A-C).Dot(u1)*u1;
514
double pB = (B-A).Dot(u1);
515
double pC = (C+E-A).Dot(u1);
516
double pD = (D+E-A).Dot(u1);
518
double minofmaxes = min(max(pA,pB),max(pC,pD));
519
double maxofmins = max(min(pA,pB),min(pC,pD));
521
AB = A + u1*(minofmaxes+maxofmins)/2;
528
//---------------------------------------------------------------------------
529
double CUtils::LineDistance(CVec A, CVec B, CVec C, CVec D)
530
//---------------------------------------------------------------------------
531
{// Find distance betweenn two skew lines AB and CD in space.
535
// Unit vector U1 along Line 1:
536
// U1 = (B-A)/| B-A |
537
CVec u1 = (B-A).Normalized();
538
// Unit vector U2 along Line 2:
539
// U2 = (D-C)/| D-C |
540
CVec u2 = (D-C).Normalized();
541
// Unit vector U3 from Line 2 toward Line 1:
542
// U3 = (U1 cross U2)/| U1 cross U2 |
543
CVec u3 = (u1.Cross(u2)).Normalized();
544
// Check for parallel lines
545
if (u3.x < 1E-4 && u3.x > -1E-4 &&
546
u3.y < 1E-4 && u3.y > -1E-4 &&
547
u3.z < 1E-4 && u3.z > -1E-4) {
548
d = ((A-C) - (A-C).Dot(u2)*u2).Length(); // distance between parallel
550
// Displacement vector from Line 2 to Line 1:
551
// E = ((A-C) dot U3) U3
559
//---------------------------------------------------------------------------
560
CString CUtils::GetPath(CString filename)
561
//---------------------------------------------------------------------------
562
{ // return string contaiing path portion of a complex filename
564
int p = max(filename.ReverseFind('\\'), filename.ReverseFind('/'));
568
// fix: if filename is already a path, then just return it
569
int t = filename.ReverseFind('.');
573
return filename.Left(p);
577
//---------------------------------------------------------------------------
578
CString CUtils::RemoveComment(CString str)
579
//---------------------------------------------------------------------------
580
{ // remove comment from the string
581
CString delin = "#%/ \t";
582
CString result = str.SpanExcluding(delin);
586
//---------------------------------------------------------------------------
587
long CUtils::Round(double x)
588
//---------------------------------------------------------------------------
590
ASSERT(x >= LONG_MIN-0.5);
591
ASSERT(x <= LONG_MAX+0.5);
593
return (long) (x+0.5);
594
return (long) (x-0.5);
added
removed
software/projects/FabStudio v0/Fab@Home Studio/Utils.cpp
