29
29
#include < assert.h>
30
30
#define ROTATE (a,i,j,k,l ) g=a[i][j];h=a[k][l];a[i][j]=g-s*(h+g*tau);a[k][l]=h+s*(g-h*tau);
31
31
32
+ // used to select solving method in calcPoint()
32
33
int method = 3 ;
33
34
34
35
// for reducing two upper triangular systems of equations into 1
@@ -506,62 +507,56 @@ void descent ( float A[][3], float B[], float guess[], BoundingBoxf *box )
506
507
store [ 1 ] = guess [ 1 ];
507
508
store [ 2 ] = guess [ 2 ];
508
509
509
- if ( method == 2 || method == 0 )
510
- {
510
+ if ( method == 2 || method == 0 ) {
511
511
512
- i = 0 ;
513
- r [ 0 ] = B [ 0 ] - ( A [ 0 ] [ 0 ] * guess [ 0 ] + A [ 0 ] [ 1 ] * guess [ 1 ] + A [ 0 ] [ 2 ] * guess [ 2 ] );
514
- r [ 1 ] = B [ 1 ] - ( A [ 1 ] [ 0 ] * guess [ 0 ] + A [ 1 ] [ 1 ] * guess [ 1 ] + A [ 1 ] [ 2 ] * guess [ 2 ] );
515
- r [ 2 ] = B [ 2 ] - ( A [ 2 ] [ 0 ] * guess [ 0 ] + A [ 2 ] [ 1 ] * guess [ 1 ] + A [ 2 ] [ 2 ] * guess [ 2 ] );
512
+ i = 0 ;
513
+ r [ 0 ] = B [ 0 ] - ( A [ 0 ] [ 0 ] * guess [ 0 ] + A [ 0 ] [ 1 ] * guess [ 1 ] + A [ 0 ] [ 2 ] * guess [ 2 ] );
514
+ r [ 1 ] = B [ 1 ] - ( A [ 1 ] [ 0 ] * guess [ 0 ] + A [ 1 ] [ 1 ] * guess [ 1 ] + A [ 1 ] [ 2 ] * guess [ 2 ] );
515
+ r [ 2 ] = B [ 2 ] - ( A [ 2 ] [ 0 ] * guess [ 0 ] + A [ 2 ] [ 1 ] * guess [ 1 ] + A [ 2 ] [ 2 ] * guess [ 2 ] );
516
516
517
- delta = r [ 0 ] * r [ 0 ] + r [ 1 ] * r [ 1 ] + r [ 2 ] * r [ 2 ];
518
- delta0 = delta * TOLERANCE * TOLERANCE;
517
+ delta = r [ 0 ] * r [ 0 ] + r [ 1 ] * r [ 1 ] + r [ 2 ] * r [ 2 ];
518
+ delta0 = delta * TOLERANCE * TOLERANCE;
519
519
520
- while ( i < n && delta > delta0 )
521
- {
522
- div = r [ 0 ] * ( A [ 0 ] [ 0 ] * r [ 0 ] + A [ 0 ] [ 1 ] * r [ 1 ] + A [ 0 ] [ 2 ] * r [ 2 ] );
523
- div += r [ 1 ] * ( A [ 1 ] [ 0 ] * r [ 0 ] + A [ 1 ] [ 1 ] * r [ 1 ] + A [ 1 ] [ 2 ] * r [ 2 ] );
524
- div += r [ 2 ] * ( A [ 2 ] [ 0 ] * r [ 0 ] + A [ 2 ] [ 1 ] * r [ 1 ] + A [ 2 ] [ 2 ] * r [ 2 ] );
520
+ while ( i < n && delta > delta0 ) {
521
+ div = r [ 0 ] * ( A [ 0 ] [ 0 ] * r [ 0 ] + A [ 0 ] [ 1 ] * r [ 1 ] + A [ 0 ] [ 2 ] * r [ 2 ] );
522
+ div += r [ 1 ] * ( A [ 1 ] [ 0 ] * r [ 0 ] + A [ 1 ] [ 1 ] * r [ 1 ] + A [ 1 ] [ 2 ] * r [ 2 ] );
523
+ div += r [ 2 ] * ( A [ 2 ] [ 0 ] * r [ 0 ] + A [ 2 ] [ 1 ] * r [ 1 ] + A [ 2 ] [ 2 ] * r [ 2 ] );
525
524
526
- if ( fabs ( div ) < 0 .0000001f )
527
- {
528
- break ;
529
- }
525
+ if ( fabs ( div ) < 0 .0000001f )
526
+ break ;
530
527
531
- alpha = delta / div ;
528
+ alpha = delta / div ;
532
529
533
- newPoint [ 0 ] = guess [ 0 ] + alpha * r [ 0 ];
534
- newPoint [ 1 ] = guess [ 1 ] + alpha * r [ 1 ];
535
- newPoint [ 2 ] = guess [ 2 ] + alpha * r [ 2 ];
530
+ newPoint [ 0 ] = guess [ 0 ] + alpha * r [ 0 ];
531
+ newPoint [ 1 ] = guess [ 1 ] + alpha * r [ 1 ];
532
+ newPoint [ 2 ] = guess [ 2 ] + alpha * r [ 2 ];
536
533
537
- guess [ 0 ] = newPoint [ 0 ];
538
- guess [ 1 ] = newPoint [ 1 ];
539
- guess [ 2 ] = newPoint [ 2 ];
534
+ guess [ 0 ] = newPoint [ 0 ];
535
+ guess [ 1 ] = newPoint [ 1 ];
536
+ guess [ 2 ] = newPoint [ 2 ];
540
537
541
- r [ 0 ] = B [ 0 ] - ( A [ 0 ] [ 0 ] * guess [ 0 ] + A [ 0 ] [ 1 ] * guess [ 1 ] + A [ 0 ] [ 2 ] * guess [ 2 ] );
542
- r [ 1 ] = B [ 1 ] - ( A [ 1 ] [ 0 ] * guess [ 0 ] + A [ 1 ] [ 1 ] * guess [ 1 ] + A [ 1 ] [ 2 ] * guess [ 2 ] );
543
- r [ 2 ] = B [ 2 ] - ( A [ 2 ] [ 0 ] * guess [ 0 ] + A [ 2 ] [ 1 ] * guess [ 1 ] + A [ 2 ] [ 2 ] * guess [ 2 ] );
538
+ r [ 0 ] = B [ 0 ] - ( A [ 0 ] [ 0 ] * guess [ 0 ] + A [ 0 ] [ 1 ] * guess [ 1 ] + A [ 0 ] [ 2 ] * guess [ 2 ] );
539
+ r [ 1 ] = B [ 1 ] - ( A [ 1 ] [ 0 ] * guess [ 0 ] + A [ 1 ] [ 1 ] * guess [ 1 ] + A [ 1 ] [ 2 ] * guess [ 2 ] );
540
+ r [ 2 ] = B [ 2 ] - ( A [ 2 ] [ 0 ] * guess [ 0 ] + A [ 2 ] [ 1 ] * guess [ 1 ] + A [ 2 ] [ 2 ] * guess [ 2 ] );
544
541
545
- delta = r [ 0 ] * r [ 0 ] + r [ 1 ] * r [ 1 ] + r [ 2 ] * r [ 2 ];
542
+ delta = r [ 0 ] * r [ 0 ] + r [ 1 ] * r [ 1 ] + r [ 2 ] * r [ 2 ];
546
543
547
- i++;
548
- }
544
+ i++;
545
+ }
549
546
550
- if ( guess [ 0 ] >= box->begin .x && guess [ 0 ] <= box->end .x &&
551
- guess [ 1 ] >= box->begin .y && guess [ 1 ] <= box->end .y &&
552
- guess [ 2 ] >= box->begin .z && guess [ 2 ] <= box->end .z )
553
- {
554
- return ;
555
- }
556
- }
547
+ if ( guess [ 0 ] >= box->begin .x && guess [ 0 ] <= box->end .x &&
548
+ guess [ 1 ] >= box->begin .y && guess [ 1 ] <= box->end .y &&
549
+ guess [ 2 ] >= box->begin .z && guess [ 2 ] <= box->end .z )
550
+ {
551
+ return ;
552
+ }
553
+ } // method 2 or 0
557
554
558
- if ( method == 0 || method == 1 )
559
- {
555
+ if ( method == 0 || method == 1 ) {
560
556
c = A [ 0 ] [ 0 ] + A [ 1 ] [ 1 ] + A [ 2 ] [ 2 ];
561
557
if ( c == 0 )
562
- {
563
558
return ;
564
- }
559
+
565
560
c = ( 0 .75f / c );
566
561
567
562
guess [ 0 ] = store [ 0 ];
@@ -572,8 +567,7 @@ void descent ( float A[][3], float B[], float guess[], BoundingBoxf *box )
572
567
r [ 1 ] = B [ 1 ] - ( A [ 1 ] [ 0 ] * guess [ 0 ] + A [ 1 ] [ 1 ] * guess [ 1 ] + A [ 1 ] [ 2 ] * guess [ 2 ] );
573
568
r [ 2 ] = B [ 2 ] - ( A [ 2 ] [ 0 ] * guess [ 0 ] + A [ 2 ] [ 1 ] * guess [ 1 ] + A [ 2 ] [ 2 ] * guess [ 2 ] );
574
569
575
- for ( i = 0 ; i < n; i++ )
576
- {
570
+ for ( i = 0 ; i < n; i++ ) {
577
571
guess [ 0 ] = guess [ 0 ] + c * r [ 0 ];
578
572
guess [ 1 ] = guess [ 1 ] + c * r [ 1 ];
579
573
guess [ 2 ] = guess [ 2 ] + c * r [ 2 ];
@@ -617,9 +611,9 @@ float calcPoint ( float halfA[], float b[], float btb, float midpoint[], float r
617
611
a [ 2 ] [ 1 ] = halfA [ 4 ];
618
612
a [ 2 ] [ 2 ] = halfA [ 5 ];
619
613
620
- switch ( method )
614
+ switch ( method ) // by default method = 3
621
615
{
622
- case 0 :
616
+ case 0 :
623
617
case 1 :
624
618
case 2 :
625
619
rvalue [ 0 ] = midpoint [ 0 ];
@@ -630,13 +624,10 @@ float calcPoint ( float halfA[], float b[], float btb, float midpoint[], float r
630
624
return calcError ( a, b, btb, rvalue );
631
625
break ;
632
626
case 3 :
633
- matInverse ( a, midpoint, inv, w, u );
634
-
635
-
627
+ matInverse ( a, midpoint, inv, w, u );
636
628
newB [ 0 ] = b [ 0 ] - a [ 0 ] [ 0 ] * midpoint [ 0 ] - a [ 0 ] [ 1 ] * midpoint [ 1 ] - a [ 0 ] [ 2 ] * midpoint [ 2 ];
637
629
newB [ 1 ] = b [ 1 ] - a [ 1 ] [ 0 ] * midpoint [ 0 ] - a [ 1 ] [ 1 ] * midpoint [ 1 ] - a [ 1 ] [ 2 ] * midpoint [ 2 ];
638
630
newB [ 2 ] = b [ 2 ] - a [ 2 ] [ 0 ] * midpoint [ 0 ] - a [ 2 ] [ 1 ] * midpoint [ 1 ] - a [ 2 ] [ 2 ] * midpoint [ 2 ];
639
-
640
631
rvalue [ 0 ] = inv [ 0 ] [ 0 ] * newB [ 0 ] + inv [ 1 ] [ 0 ] * newB [ 1 ] + inv [ 2 ] [ 0 ] * newB [ 2 ] + midpoint [ 0 ];
641
632
rvalue [ 1 ] = inv [ 0 ] [ 1 ] * newB [ 0 ] + inv [ 1 ] [ 1 ] * newB [ 1 ] + inv [ 2 ] [ 1 ] * newB [ 2 ] + midpoint [ 1 ];
642
633
rvalue [ 2 ] = inv [ 0 ] [ 2 ] * newB [ 0 ] + inv [ 1 ] [ 2 ] * newB [ 1 ] + inv [ 2 ] [ 2 ] * newB [ 2 ] + midpoint [ 2 ];
@@ -768,7 +759,7 @@ float calcPoint ( float halfA[], float b[], float btb, float midpoint[], float r
768
759
return ret;
769
760
770
761
break ;
771
- case 5 :
762
+ case 5 : // do nothing, return midpoint
772
763
rvalue [ 0 ] = midpoint [ 0 ];
773
764
rvalue [ 1 ] = midpoint [ 1 ];
774
765
rvalue [ 2 ] = midpoint [ 2 ];
0 commit comments