source/trianglefunctions.cc

   1 /* This file is part of Shapes.
   2  *
   3  * Shapes is free software: you can redistribute it and/or modify
   4  * it under the terms of the GNU General Public License as published by
   5  * the Free Software Foundation, either version 3 of the License, or
   6  * any later version.
   7  *
   8  * Shapes is distributed in the hope that it will be useful,
   9  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  11  * GNU General Public License for more details.
  12  *
  13  * You should have received a copy of the GNU General Public License
  14  * along with Shapes.  If not, see <http://www.gnu.org/licenses/>.
  15  *
  16  * Copyright 2008 Henrik Tidefelt
  17  */
  18
  19 #include <cmath>
  20
  21 #include "trianglefunctions.h"
  22
  23
  24 using namespace Shapes;
  25
  26
  27 Concrete::Coords2D
  28 Computation::triangleIncenter( const Concrete::Coords2D & p1, const Concrete::Coords2D & p2, const Concrete::Coords2D & p3 )
  29 {
  30         const Concrete::Coords2D d12 = p2 - p1;
  31         const Concrete::Coords2D d13 = p3 - p1;
  32         const Concrete::Coords2D d23 = p3 - p2;
  33
  34         Concrete::Length n12 = d12.norm( );
  35         Concrete::Length n13 = d13.norm( );
  36         Concrete::Length n23 = d23.norm( );
  37
  38         // All initializations must come before the goto jumps, or we would have to use one scope delimiter per norm.
  39         // However, I don't just because that would make Emacs indent in an ugly way.
  40
  41         const Concrete::Length TINY_LENGTH( 1e-8 );
  42
  43         if( n12 < TINY_LENGTH )
  44                 {
  45                         goto returnmean;
  46                 }
  47         if( n13 < TINY_LENGTH )
  48                 {
  49                         goto returnmean;
  50                 }
  51         if( n23 < TINY_LENGTH )
  52                 {
  53                         goto returnmean;
  54                 }
  55
  56         {
  57
  58                 const Concrete::UnitFloatPair r12 = d12.direction( n12 );
  59                 const Concrete::UnitFloatPair r13 = d13.direction( n13 );
  60                 const Concrete::UnitFloatPair r23 = d23.direction( n23 );
  61
  62                 // Now we cheat, because we will not make the normals unit!
  63                 const Concrete::UnitFloatPair a1( r12.x_ - r13.x_, r12.y_ - r13.y_, bool( ) );  // normal to the angle bisector line through p1
  64                 const Concrete::UnitFloatPair a2( r12.x_ + r23.x_, r12.y_ + r23.y_, bool( ) );  // normal to the angle bisector line through p2
  65
  66                 Concrete::Length b1 = Concrete::inner( a1, p1 );        // constant of the angle bisector line through p1
  67                 Concrete::Length b2 = Concrete::inner( a2, p2 );        // constant of the angle bisector line through p2
  68
  69                 // Then we compute the intersection of those lines.
  70                 double det = ( a1.x_ * a2.y_ - a1.y_ * a2.x_ );
  71                 if( fabs( det ) < 1e-8 )
  72                         {
  73                                 goto returnmean;
  74                         }
  75                 double invDet = 1. / det;
  76                 Concrete::Length x = invDet * (  a2.y_ * b1 - a1.y_ * b2 );
  77                 Concrete::Length y = invDet * ( - a2.x_ * b1 + a1.x_ * b2 );
  78
  79                 return Concrete::Coords2D( x, y );
  80         }
  81  returnmean:
  82         return (1/3) * ( p1 + p2 + p3 );
  83 }
  84
  85 Concrete::Coords3D
  86 Computation::triangleIncenter( const Concrete::Coords3D & p1, const Concrete::Coords3D & p2, const Concrete::Coords3D & p3 )
  87 {
  88         const Concrete::Coords3D d12 = p2 - p1;
  89         const Concrete::Coords3D d13 = p3 - p1;
  90         const Concrete::Coords3D d23 = p3 - p2;
  91
  92         Concrete::Length n12 = d12.norm( );
  93         Concrete::Length n13 = d13.norm( );
  94         Concrete::Length n23 = d23.norm( );
  95
  96         // All initializations must come before the goto jumps, or we would have to use one scope delimiter per norm.
  97         // However, I don't just because that would make Emacs indent in an ugly way.
  98
  99         const Concrete::Length TINY_LENGTH( 1e-8 );
 100
 101         if( n12 < TINY_LENGTH )
 102                 {
 103                         goto returnmean;
 104                 }
 105         if( n13 < TINY_LENGTH )
 106                 {
 107                         goto returnmean;
 108                 }
 109         if( n23 < TINY_LENGTH )
 110                 {
 111                         goto returnmean;
 112                 }
 113
 114         {
 115
 116                 const Concrete::UnitFloatTriple r12 = d12.direction( n12 );
 117                 const Concrete::UnitFloatTriple r13 = d13.direction( n13 );
 118                 const Concrete::UnitFloatTriple r23 = d23.direction( n23 );
 119
 120                 // Now we cheat, because we will not make the directions unit!
 121                 // Note that we don't use the same kind of the equations in 3D as we did in 2D!
 122                 const Concrete::UnitFloatTriple a1( r12.x_ + r13.x_, r12.y_ + r13.y_, r12.z_ + r13.z_, bool( ) );       // direction of the angle bisector line through p1
 123                 const Concrete::UnitFloatTriple a2( r23.x_ - r12.x_, r23.y_ - r12.y_, r23.z_ - r12.z_, bool( ) );       // direction of the angle bisector line through p2
 124
 125                 // We will now solve for the scalars k1 and k2 such that
 126                 //       p1 + k1 a1 == p2 + k2 a2
 127                 // that is, we solve the following system in least-squares sense (this gives robustness)
 128                 //       ( a1 -a2 ) ( k1 k2 ) == p2 - p1
 129                 // To do this, we form the normal equations:
 130                 //       A ( k1 k2 ) == B
 131                 // Where
 132                 //       A = Transpose( a1 -a2 ) ( a1 -a2 )
 133                 //       B = Transpose( a1 -a2 ) ( p2 - p1 ) = Transpose( a1 -a2 ) d12
 134
 135                 double a11 = Concrete::inner( a1, a1 );
 136                 double a12 = - Concrete::inner( a1, a2 ); // this is also a21
 137                 double a22 = Concrete::inner( a2, a2 );
 138
 139                 Concrete::Length b1 = Concrete::inner( a1, d12 );
 140                 Concrete::Length b2 = - Concrete::inner( a2, d12 );
 141
 142                 // Then we compute the intersection of those lines.
 143                 double det = ( a11 * a22 - a12 * a12 );
 144                 if( fabs( det ) < 1e-8 )
 145                         {
 146                                 goto returnmean;
 147                         }
 148                 double invDet = 1. / det;
 149                 Concrete::Length k1 = invDet * (         a22 * b1 - a12 * b2 );
 150
 151                 return p1 + k1 * a1;
 152         }
 153  returnmean:
 154         return (1/3) * ( p1 + p2 + p3 );
 155 }
 156
 157 Concrete::Area
 158 Computation::triangleArea( const Concrete::Coords2D & p1, const Concrete::Coords2D & p2, const Concrete::Coords2D & p3 )
 159 {
 160         const Concrete::Coords2D d1 = p2 - p1;
 161         const Concrete::Coords2D d2 = p3 - p1;
 162         return 0.5 * ( d1.x_ * d2.y_ - d1.y_ * d2.x_ ).abs( );
 163 }
 164
 165 Concrete::Length
 166 Computation::triangleSemiPerimeter( const Concrete::Coords2D & p1, const Concrete::Coords2D & p2, const Concrete::Coords2D & p3 )
 167 {
 168         return 0.5 * ( ( p2 - p1 ).norm( ) + ( p3 - p2 ).norm( ) + ( p1 - p3 ).norm( ) );
 169 }
 170
 171 Concrete::Area
 172 Computation::triangleArea( const Concrete::Coords3D & p1, const Concrete::Coords3D & p2, const Concrete::Coords3D & p3 )
 173 {
 174         return 0.5 * Concrete::crossMagnitude( p2 - p1, p3 - p1 );
 175 }