source/elementarycoords.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, 2009 Henrik Tidefelt
  17  */
  18
  19 #include "elementarytypes.h"
  20 #include "functiontypes.h"     // this is a trick to avoid #include problems...
  21 #include "elementarycoords.h"
  22
  23
  24 using namespace Shapes;
  25
  26 Concrete::UnitFloatPair
  27 Concrete::UnitFloatPair::unnormalizedScaling( double a ) const
  28 {
  29         return Concrete::UnitFloatPair( a * x_, a * y_, true );
  30 }
  31
  32 Concrete::Coords2D::Coords2D( const Lang::Coords2D & orig )
  33         : x_( orig.x_.get( ) ), y_( orig.y_.get( ) )
  34 { }
  35
  36 Concrete::Coords2D::Coords2D( const Concrete::Coords2D & base, const Lang::Coords2D & orig )
  37         : x_( orig.x_.get( base.x_ ) ), y_( orig.y_.get( base.y_ ) )
  38 { }
  39
  40 Concrete::Coords2D::operator Lang::Coords2D ( ) const
  41 {
  42         return Lang::Coords2D( Lang::Length( x_ ), Lang::Length( y_ ) );
  43 }
  44
  45 Concrete::Coords2D *
  46 Concrete::Coords2D::transformedPtr( const Lang::Transform2D & tf ) const
  47 {
  48         return new Concrete::Coords2D( tf.xx_ * x_ + tf.xy_ * y_ + tf.xt_, tf.yx_ * x_ + tf.yy_ * y_ + tf.yt_ );
  49 }
  50
  51 Concrete::Coords2D
  52 Concrete::Coords2D::transformed( const Lang::Transform2D & tf ) const
  53 {
  54         return Concrete::Coords2D( tf.xx_ * x_ + tf.xy_ * y_ + tf.xt_, tf.yx_ * x_ + tf.yy_ * y_ + tf.yt_ );
  55 }
  56
  57 Concrete::Coords3D *
  58 Concrete::Coords2D::transformedPtr( const Lang::Transform3D & tf ) const
  59 {
  60         return new Concrete::Coords3D( tf.xx_ * x_ + tf.xy_ * y_ + tf.xt_,
  61                                                                                                                                                                         tf.yx_ * x_ + tf.yy_ * y_ + tf.yt_,
  62                                                                                                                                                                         tf.zx_ * x_ + tf.zy_ * y_ + tf.zt_ );
  63 }
  64
  65 Concrete::Length
  66 Concrete::Coords2D::norm( ) const
  67 {
  68         return hypotPhysical( x_, y_ );
  69 }
  70
  71 double
  72 Concrete::Coords2D::normScalar( ) const
  73 {
  74         return hypotPhysical( x_, y_ ).offtype< 1, 0 >( );
  75 }
  76
  77 Concrete::UnitFloatPair
  78 Concrete::Coords2D::direction( ) const
  79 {
  80         return Concrete::UnitFloatPair( x_.offtype< 1, 0 >( ), y_.offtype< 1, 0 >( ) );
  81 }
  82
  83 double
  84 Concrete::Coords2D::angleCosineSignSquared( const Concrete::Coords2D & b ) const
  85 {
  86         /* Squaring the angle cosine fraction with sign is basically a matter of squaring numerator and denominator, keeping the sign of the numerator
  87          * (as the sign of the denominator is always positive).
  88          * Since the denominator is the product of the legths of the vectors, squaring the denominator yields the product of the lengths squared.
  89          */
  90         double num = Concrete::Length::offtype( x_ ) * Concrete::Length::offtype( b.x_ ) + Concrete::Length::offtype( y_ ) * Concrete::Length::offtype( b.y_ );
  91         double den2 =
  92                 ( Concrete::Length::offtype( x_ ) * Concrete::Length::offtype( x_ ) + Concrete::Length::offtype( y_ ) * Concrete::Length::offtype( y_ ) )
  93                 *
  94                 ( Concrete::Length::offtype( b.x_ ) * Concrete::Length::offtype( b.x_ ) + Concrete::Length::offtype( b.y_ ) * Concrete::Length::offtype( b.y_ ) );
  95         return num * ( num >= 0 ? num : -num ) / den2;
  96 }
  97
  98 Concrete::UnitFloatPair
  99 Concrete::Coords2D::direction( Concrete::Length precomputedLength ) const
 100 {
 101         return Concrete::UnitFloatPair( x_.offtype< 1, 0 >( ), y_.offtype< 1, 0 >( ), precomputedLength.offtype< 1, 0 >( ) );
 102 }
 103
 104
 105 std::ostream &
 106 Concrete::operator << ( std::ostream & os, const Concrete::Coords2D & self )
 107 {
 108         static char buf[25];
 109         sprintf( buf, "%.2f %.2f", double( self.x_.offtype< 1, 0 >( ) ), double( self.y_.offtype< 1, 0 >( ) ) );
 110         os << buf ;
 111         return os;
 112 }
 113
 114 Concrete::Coords2D
 115 Concrete::operator * ( double scalar, const Concrete::Coords2D & coords )
 116 {
 117         return Concrete::Coords2D( scalar * coords.x_, scalar * coords.y_ );
 118 }
 119
 120 Concrete::Coords2D
 121 Concrete::operator * ( const Concrete::Coords2D & coords, double scalar )
 122 {
 123         return Concrete::Coords2D( scalar * coords.x_, scalar * coords.y_ );
 124 }
 125
 126 Concrete::Coords2D
 127 Concrete::operator + ( const Concrete::Coords2D & coords1, const Concrete::Coords2D & coords2 )
 128 {
 129         return Concrete::Coords2D( coords1.x_ + coords2.x_, coords1.y_ + coords2.y_ );
 130 }
 131
 132 Concrete::Coords2D
 133 Concrete::operator - ( const Concrete::Coords2D & coords1, const Concrete::Coords2D & coords2 )
 134 {
 135         return Concrete::Coords2D( coords1.x_ - coords2.x_, coords1.y_ - coords2.y_ );
 136 }
 137
 138
 139
 140 Concrete::Coords2D
 141 Concrete::operator * ( Concrete::Length length, const Concrete::UnitFloatPair & direction )
 142 {
 143         return Concrete::Coords2D( length * direction.x_, length * direction.y_ );
 144 }
 145
 146 Concrete::Coords2D
 147 Concrete::operator * ( const Concrete::UnitFloatPair & direction, Concrete::Length length )
 148 {
 149         return Concrete::Coords2D( length * direction.x_, length * direction.y_ );
 150 }
 151
 152 // Concrete::UnitFloatPair
 153 // Concrete::operator + ( const Concrete::UnitFloatPair & coords1, const Concrete::UnitFloatPair & coords2 )
 154 // {
 155 //       return Concrete::UnitFloatPair( coords1.x_ + coords2.x_, coords1.y_ + coords2.y_ );
 156 // }
 157
 158 // Concrete::UnitFloatPair
 159 // Concrete::operator - ( const Concrete::UnitFloatPair & coords1, const Concrete::UnitFloatPair & coords2 )
 160 // {
 161 //       return Concrete::UnitFloatPair( coords1.x_ - coords2.x_, coords1.y_ - coords2.y_ );
 162 // }
 163
 164
 165
 166 Concrete::Coords3D *
 167 Concrete::Coords3D::transformedPtr( const Lang::Transform3D & tf ) const
 168 {
 169         return new Concrete::Coords3D( tf.xx_ * x_ + tf.xy_ * y_ + tf.xz_ * z_ + tf.xt_,
 170                                                                                                                                  tf.yx_ * x_ + tf.yy_ * y_ + tf.yz_ * z_ + tf.yt_,
 171                                                                                                                                  tf.zx_ * x_ + tf.zy_ * y_ + tf.zz_ * z_ + tf.zt_ );
 172 }
 173
 174 Concrete::Coords3D
 175 Concrete::Coords3D::transformed( const Lang::Transform3D & tf ) const
 176 {
 177         return Concrete::Coords3D( tf.xx_ * x_ + tf.xy_ * y_ + tf.xz_ * z_ + tf.xt_,
 178                                                                                                                  tf.yx_ * x_ + tf.yy_ * y_ + tf.yz_ * z_ + tf.yt_,
 179                                                                                                                  tf.zx_ * x_ + tf.zy_ * y_ + tf.zz_ * z_ + tf.zt_ );
 180 }
 181
 182 Concrete::Coords2D *
 183 Concrete::Coords3D::make2D( Concrete::Length eyez ) const
 184 {
 185         if( eyez == HUGE_LENGTH )
 186                 {
 187                         return new Concrete::Coords2D( x_, y_ );
 188                 }
 189
 190         return new Concrete::Coords2D( x_ * ( eyez / ( eyez - z_ ) ),
 191                                                                                                                                  y_ * ( eyez / ( eyez - z_ ) ) );
 192 }
 193
 194 Concrete::Coords2D
 195 Concrete::Coords3D::make2DAutomatic( Concrete::Length eyez ) const
 196 {
 197         if( eyez == HUGE_LENGTH )
 198                 {
 199                         return Concrete::Coords2D( x_, y_ );
 200                 }
 201
 202         return Concrete::Coords2D( x_ * eyez / ( eyez - z_ ),
 203                                                                                                                  y_ * eyez / ( eyez - z_ ) );
 204 }
 205
 206 std::ostream &
 207 Concrete::operator << ( std::ostream & os, const Concrete::Coords3D & self )
 208 {
 209         static char buf[38];
 210         sprintf( buf, "%.2f %.2f %.2f", double( self.x_.offtype< 1, 0 >( ) ), double( self.y_.offtype< 1, 0 >( ) ), double( self.z_.offtype< 1, 0 >( ) ) );
 211         os << buf ;
 212         return os;
 213 }
 214
 215 Concrete::Coords3D::Coords3D( const Lang::Coords3D & orig )
 216         : x_( orig.x_.getScalar( ) ), y_( orig.y_.getScalar( ) ), z_( orig.z_.getScalar( ) )
 217 { }
 218
 219 Concrete::Coords3D::Coords3D( const Concrete::Coords3D & base, const Lang::Coords3D & orig )
 220         : x_( orig.x_.getScalar( base.x_ ) ), y_( orig.y_.getScalar( base.y_ ) ), z_( orig.z_.getScalar( base.z_ ) )
 221 { }
 222
 223 Concrete::Coords3D::operator Lang::Coords3D ( ) const
 224 {
 225         return Lang::Coords3D( Lang::Length( x_ ), Lang::Length( y_ ), Lang::Length( z_ ) );
 226 }
 227
 228
 229 Concrete::Coords3D
 230 Concrete::operator * ( double scalar, const Concrete::Coords3D & coords )
 231 {
 232         return Concrete::Coords3D( scalar * coords.x_, scalar * coords.y_, scalar * coords.z_ );
 233 }
 234
 235 Concrete::Coords3D
 236 Concrete::operator * ( const Concrete::Coords3D & coords, double scalar )
 237 {
 238         return Concrete::Coords3D( scalar * coords.x_, scalar * coords.y_, scalar * coords.z_ );
 239 }
 240
 241 Concrete::Coords3D
 242 Concrete::operator + ( const Concrete::Coords3D & coords1, const Concrete::Coords3D & coords2 )
 243 {
 244         return Concrete::Coords3D( coords1.x_ + coords2.x_, coords1.y_ + coords2.y_, coords1.z_ + coords2.z_ );
 245 }
 246
 247 Concrete::Coords3D
 248 Concrete::operator - ( const Concrete::Coords3D & coords1, const Concrete::Coords3D & coords2 )
 249 {
 250         return Concrete::Coords3D( coords1.x_ - coords2.x_, coords1.y_ - coords2.y_, coords1.z_ - coords2.z_ );
 251 }
 252
 253 double
 254 Concrete::Coords3D::normalizedInner( const Concrete::Coords3D & b ) const
 255 {
 256         return ( ( x_ * b.x_ + y_ * b.y_ + z_ * b.z_ ) / hypotPhysical( x_, y_, z_ ) ).offtype< 1, 0 >( );
 257 }
 258
 259 Concrete::Length
 260 Concrete::Coords3D::norm( ) const
 261 {
 262         return hypotPhysical( x_, y_, z_ );
 263 }
 264
 265 double
 266 Concrete::Coords3D::normScalar( ) const
 267 {
 268         return hypotPhysical( x_, y_, z_ ).offtype< 1, 0 >( );
 269 }
 270
 271 Concrete::UnitFloatTriple
 272 Concrete::Coords3D::direction( ) const
 273 {
 274         return Concrete::UnitFloatTriple( x_.offtype< 1, 0 >( ), y_.offtype< 1, 0 >( ), z_.offtype< 1, 0 >( ) );
 275 }
 276
 277 Concrete::UnitFloatTriple
 278 Concrete::Coords3D::directionNoFail( ) const
 279 {
 280         try
 281                 {
 282                         return direction( );
 283                 }
 284         catch( Exceptions::InternalError & ball )
 285                 {
 286                         return UnitFloatTriple( bool( ) );
 287                 }
 288 }
 289
 290 Concrete::UnitFloatTriple
 291 Concrete::Coords3D::direction( Concrete::Length precomputedLength ) const
 292 {
 293         return Concrete::UnitFloatTriple( x_.offtype< 1, 0 >( ), y_.offtype< 1, 0 >( ), z_.offtype< 1, 0 >( ), precomputedLength.offtype< 1, 0 >( ) );
 294 }
 295
 296
 297
 298 Concrete::UnitFloatTriple::UnitFloatTriple( double x, double y, double z )
 299 {
 300         double len = Concrete::Scalar::hypot3( x, y, z );
 301         if( len < 1e-5 )
 302                 {
 303                         throw Exceptions::InternalError( "A concrete UnitFloatTriple was initialized with a very short vector." );
 304                 }
 305         double tmp = 1. / Concrete::Scalar::hypot3( x, y, z );
 306         x_ = tmp * x;
 307         y_ = tmp * y;
 308         z_ = tmp * z;
 309 }
 310
 311 Concrete::UnitFloatTriple
 312 Concrete::UnitFloatTriple::unnormalizedScaling( double a ) const
 313 {
 314         return Concrete::UnitFloatTriple( a * x_, a * y_, a * z_, true );
 315 }
 316
 317
 318 Concrete::UnitFloatTriple
 319 Concrete::UnitFloatTriple::reflect( const Concrete::UnitFloatTriple dir ) const
 320 {
 321         double a = 2 * Concrete::inner( *this, dir );
 322         return Concrete::UnitFloatTriple( a * x_ - dir.x_,
 323                                                                                                                                                 a * y_ - dir.y_,
 324                                                                                                                                                 a * z_ - dir.z_,
 325                                                                                                                                                 bool( ) );      // the bool( ) means it's already normalized
 326 }
 327
 328 Concrete::UnitFloatTriple
 329 Concrete::UnitFloatTriple::rotate( const Concrete::UnitFloatTriple dir, const double angle ) const
 330 {
 331         // The following may not be efficient, but it was easy to code by copying code from Core_rotate3D.
 332         // I've got the feeling that this is much better done using geometric algebra based on Clifford algebra.
 333
 334         double x = dir.x_;
 335         double y = dir.y_;
 336         double z = dir.z_;
 337         double x2 = x * x;
 338         double y2 = y * y;
 339         double z2 = z * z;
 340         double c = cos( angle );
 341         double s = sin( angle );
 342         return Concrete::UnitFloatTriple( (x2+(y2+z2)*c)*x_ + (x*y*(1-c)+z*s)*y_ + (x*z*(1-c)-y*s)*z_,
 343                                                                                                                                                 (x*y*(1-c)-z*s)*x_ + (y2+(x2+z2)*c)*y_ + (y*z*(1-c)+x*s)*z_,
 344                                                                                                                                                 (x*z*(1-c)+y*s)*x_ + (y*z*(1-c)-x*s)*y_ + (z2+(x2+y2)*c)*z_,
 345                                                                                                                                                 bool( ) );
 346 }
 347
 348
 349
 350 Concrete::Coords3D
 351 Concrete::operator * ( Concrete::Length length, const Concrete::UnitFloatTriple & direction )
 352 {
 353         return Concrete::Coords3D( length * direction.x_, length * direction.y_, length * direction.z_ );
 354 }
 355
 356 Concrete::Coords3D
 357 Concrete::operator * ( const Concrete::UnitFloatTriple & direction, Concrete::Length length )
 358 {
 359         return Concrete::Coords3D( length * direction.x_, length * direction.y_, length * direction.z_ );
 360 }
 361
 362 // Concrete::UnitFloatTriple
 363 // Concrete::operator + ( const Concrete::UnitFloatTriple & coords1, const Concrete::UnitFloatTriple & coords2 )
 364 // {
 365 //       return Concrete::UnitFloatTriple( coords1.x_ + coords2.x_, coords1.y_ + coords2.y_, coords1.z_ + coords2.z_ );
 366 // }
 367
 368 // Concrete::UnitFloatTriple
 369 // Concrete::operator - ( const Concrete::UnitFloatTriple & coords1, const Concrete::UnitFloatTriple & coords2 )
 370 // {
 371 //       return Concrete::UnitFloatTriple( coords1.x_ - coords2.x_, coords1.y_ - coords2.y_, coords1.z_ - coords2.z_ );
 372 // }
 373
 374 Concrete::Area
 375 Concrete::crossMagnitude( const Concrete::Coords3D & a, const Concrete::Coords3D & b )
 376 {
 377         return hypotPhysical( a.y_ * b.z_ - a.z_ * b.y_,
 378                                                                                                 a.z_ * b.x_ - a.x_ * b.z_,
 379                                                                                                 a.x_ * b.y_ - a.y_ * b.x_ );
 380 }