diff options
Diffstat (limited to 'tool_src/BspLib/Transform3.cpp')
| -rw-r--r-- | tool_src/BspLib/Transform3.cpp | 312 |
1 files changed, 312 insertions, 0 deletions
diff --git a/tool_src/BspLib/Transform3.cpp b/tool_src/BspLib/Transform3.cpp new file mode 100644 index 0000000..7f8b54d --- /dev/null +++ b/tool_src/BspLib/Transform3.cpp @@ -0,0 +1,312 @@ +//----------------------------------------------------------------------------- +// BSPLIB MODULE: Transform3.cpp +// +// Copyright (c) 1997-1998 by Markus Hadwiger +// All Rights Reserved. +//----------------------------------------------------------------------------- + +// bsplib headers +#include "Transform3.h" + + +BSPLIB_NAMESPACE_BEGIN + + +// multiply two 4x4 matrices -------------------------------------------------- +// +PRIVATE +void mat4x4_mul( double dest_matrix[][4], const double matrix1[][4], const double matrix2[][4] ) +{ + //NOTE: + // full homogeneous multiplication is performed. (that is, all 16 + // elements are calculated.) normally this isn't necessary as + // most matrices are affine. but since this is no real-time + // application more flexibility seems better. +/* + for ( int i = 0; i < 4; i++ ) { + for ( int j = 0; j < 4; j++ ) { + dest_matrix[ i ][ j ] = 0.0; + for ( int k = 0; k < 4; k++ ) + dest_matrix[ i ][ j ] += matrix1[ i ][ k ] * matrix2[ k ][ j ]; + } + } +*/ + dest_matrix[ 0 ][ 0 ] = matrix1[ 0 ][ 0 ] * matrix2[ 0 ][ 0 ] + + matrix1[ 0 ][ 1 ] * matrix2[ 1 ][ 0 ] + + matrix1[ 0 ][ 2 ] * matrix2[ 2 ][ 0 ] + + matrix1[ 0 ][ 3 ] * matrix2[ 3 ][ 0 ]; + dest_matrix[ 0 ][ 1 ] = matrix1[ 0 ][ 0 ] * matrix2[ 0 ][ 1 ] + + matrix1[ 0 ][ 1 ] * matrix2[ 1 ][ 1 ] + + matrix1[ 0 ][ 2 ] * matrix2[ 2 ][ 1 ] + + matrix1[ 0 ][ 3 ] * matrix2[ 3 ][ 1 ]; + dest_matrix[ 0 ][ 2 ] = matrix1[ 0 ][ 0 ] * matrix2[ 0 ][ 2 ] + + matrix1[ 0 ][ 1 ] * matrix2[ 1 ][ 2 ] + + matrix1[ 0 ][ 2 ] * matrix2[ 2 ][ 2 ] + + matrix1[ 0 ][ 3 ] * matrix2[ 3 ][ 2 ]; + dest_matrix[ 0 ][ 3 ] = matrix1[ 0 ][ 0 ] * matrix2[ 0 ][ 3 ] + + matrix1[ 0 ][ 1 ] * matrix2[ 1 ][ 3 ] + + matrix1[ 0 ][ 2 ] * matrix2[ 2 ][ 3 ] + + matrix1[ 0 ][ 3 ] * matrix2[ 3 ][ 3 ]; + + dest_matrix[ 1 ][ 0 ] = matrix1[ 1 ][ 0 ] * matrix2[ 0 ][ 0 ] + + matrix1[ 1 ][ 1 ] * matrix2[ 1 ][ 0 ] + + matrix1[ 1 ][ 2 ] * matrix2[ 2 ][ 0 ] + + matrix1[ 1 ][ 3 ] * matrix2[ 3 ][ 0 ]; + dest_matrix[ 1 ][ 1 ] = matrix1[ 1 ][ 0 ] * matrix2[ 0 ][ 1 ] + + matrix1[ 1 ][ 1 ] * matrix2[ 1 ][ 1 ] + + matrix1[ 1 ][ 2 ] * matrix2[ 2 ][ 1 ] + + matrix1[ 1 ][ 3 ] * matrix2[ 3 ][ 1 ]; + dest_matrix[ 1 ][ 2 ] = matrix1[ 1 ][ 0 ] * matrix2[ 0 ][ 2 ] + + matrix1[ 1 ][ 1 ] * matrix2[ 1 ][ 2 ] + + matrix1[ 1 ][ 2 ] * matrix2[ 2 ][ 2 ] + + matrix1[ 1 ][ 3 ] * matrix2[ 3 ][ 2 ]; + dest_matrix[ 1 ][ 3 ] = matrix1[ 1 ][ 0 ] * matrix2[ 0 ][ 3 ] + + matrix1[ 1 ][ 1 ] * matrix2[ 1 ][ 3 ] + + matrix1[ 1 ][ 2 ] * matrix2[ 2 ][ 3 ] + + matrix1[ 1 ][ 3 ] * matrix2[ 3 ][ 3 ]; + + dest_matrix[ 2 ][ 0 ] = matrix1[ 2 ][ 0 ] * matrix2[ 0 ][ 0 ] + + matrix1[ 2 ][ 1 ] * matrix2[ 1 ][ 0 ] + + matrix1[ 2 ][ 2 ] * matrix2[ 2 ][ 0 ] + + matrix1[ 2 ][ 3 ] * matrix2[ 3 ][ 0 ]; + dest_matrix[ 2 ][ 1 ] = matrix1[ 2 ][ 0 ] * matrix2[ 0 ][ 1 ] + + matrix1[ 2 ][ 1 ] * matrix2[ 1 ][ 1 ] + + matrix1[ 2 ][ 2 ] * matrix2[ 2 ][ 1 ] + + matrix1[ 2 ][ 3 ] * matrix2[ 3 ][ 1 ]; + dest_matrix[ 2 ][ 2 ] = matrix1[ 2 ][ 0 ] * matrix2[ 0 ][ 2 ] + + matrix1[ 2 ][ 1 ] * matrix2[ 1 ][ 2 ] + + matrix1[ 2 ][ 2 ] * matrix2[ 2 ][ 2 ] + + matrix1[ 2 ][ 3 ] * matrix2[ 3 ][ 2 ]; + dest_matrix[ 2 ][ 3 ] = matrix1[ 2 ][ 0 ] * matrix2[ 0 ][ 3 ] + + matrix1[ 2 ][ 1 ] * matrix2[ 1 ][ 3 ] + + matrix1[ 2 ][ 2 ] * matrix2[ 2 ][ 3 ] + + matrix1[ 2 ][ 3 ] * matrix2[ 3 ][ 3 ]; + + dest_matrix[ 3 ][ 0 ] = matrix1[ 3 ][ 0 ] * matrix2[ 0 ][ 0 ] + + matrix1[ 3 ][ 1 ] * matrix2[ 1 ][ 0 ] + + matrix1[ 3 ][ 2 ] * matrix2[ 2 ][ 0 ] + + matrix1[ 3 ][ 3 ] * matrix2[ 3 ][ 0 ]; + dest_matrix[ 3 ][ 1 ] = matrix1[ 3 ][ 0 ] * matrix2[ 0 ][ 1 ] + + matrix1[ 3 ][ 1 ] * matrix2[ 1 ][ 1 ] + + matrix1[ 3 ][ 2 ] * matrix2[ 2 ][ 1 ] + + matrix1[ 3 ][ 3 ] * matrix2[ 3 ][ 1 ]; + dest_matrix[ 3 ][ 2 ] = matrix1[ 3 ][ 0 ] * matrix2[ 0 ][ 2 ] + + matrix1[ 3 ][ 1 ] * matrix2[ 1 ][ 2 ] + + matrix1[ 3 ][ 2 ] * matrix2[ 2 ][ 2 ] + + matrix1[ 3 ][ 3 ] * matrix2[ 3 ][ 2 ]; + dest_matrix[ 3 ][ 3 ] = matrix1[ 3 ][ 0 ] * matrix2[ 0 ][ 3 ] + + matrix1[ 3 ][ 1 ] * matrix2[ 1 ][ 3 ] + + matrix1[ 3 ][ 2 ] * matrix2[ 2 ][ 3 ] + + matrix1[ 3 ][ 3 ] * matrix2[ 3 ][ 3 ]; +} + + +// calculate concatenation; don't overwrite object matrix --------------------- +// +Transform3 operator *( const Transform3& trafo1, const Transform3& trafo2 ) +{ + Transform3 temp; + mat4x4_mul( temp.m_matrix, trafo1.m_matrix, trafo2.m_matrix ); + return temp; +} + + +// calculate concatenation as new object matrix ------------------------------- +// +Transform3& Transform3::Concat( const Transform3& cattrafo ) +{ + Transform3 temp; + mat4x4_mul( temp.m_matrix, (const double(*)[4]) m_matrix, cattrafo.m_matrix ); + memcpy( m_matrix, temp.m_matrix, sizeof( m_matrix ) ); + return *this; +} + + +// reversed concatenation ----------------------------------------------------- +// +Transform3& Transform3::ConcatR( const Transform3& cattrafo ) +{ + Transform3 temp; + mat4x4_mul( temp.m_matrix, cattrafo.m_matrix, (const double(*)[4]) m_matrix ); + memcpy( m_matrix, temp.m_matrix, sizeof( m_matrix ) ); + return *this; +} + + +// transform Vector3 by matrix (homogeneous component is also evaluated!) ----- +// +Vector3 Transform3::TransformVector3( const Vector3& vec ) const +{ + Vector3 temp; + temp.setX( m_matrix[ 0 ][ 0 ] * vec.getX() + m_matrix[ 1 ][ 0 ] * vec.getY() + m_matrix[ 2 ][ 0 ] * vec.getZ() + m_matrix[ 3 ][ 0 ] * vec.getW() ); + temp.setY( m_matrix[ 0 ][ 1 ] * vec.getX() + m_matrix[ 1 ][ 1 ] * vec.getY() + m_matrix[ 2 ][ 1 ] * vec.getZ() + m_matrix[ 3 ][ 1 ] * vec.getW() ); + temp.setZ( m_matrix[ 0 ][ 2 ] * vec.getX() + m_matrix[ 1 ][ 2 ] * vec.getY() + m_matrix[ 2 ][ 2 ] * vec.getZ() + m_matrix[ 3 ][ 2 ] * vec.getW() ); + temp.setW( m_matrix[ 0 ][ 3 ] * vec.getX() + m_matrix[ 1 ][ 3 ] * vec.getY() + m_matrix[ 2 ][ 3 ] * vec.getZ() + m_matrix[ 3 ][ 3 ] * vec.getW() ); + return temp; +} + + +// fetch translation part of matrix ------------------------------------------- +// +Vector3 Transform3::FetchTranslation() const +{ + // return translation vector + return Vector3( m_matrix[ 3 ][ 0 ], m_matrix[ 3 ][ 1 ], m_matrix[ 3 ][ 2 ] ); +} + + +// extract translation part of matrix; set to NULL translation afterwards ----- +// +Vector3 Transform3::ExtractTranslation() +{ + // create translation vector + Vector3 temp( m_matrix[ 3 ][ 0 ], m_matrix[ 3 ][ 1 ], m_matrix[ 3 ][ 2 ] ); + // zero translation part of matrix + m_matrix[ 3 ][ 0 ] = 0.0; + m_matrix[ 3 ][ 1 ] = 0.0; + m_matrix[ 3 ][ 2 ] = 0.0; + return temp; +} + + +// create rotation matrix ----------------------------------------------------- +// +Transform3& Transform3::LoadRotation( double angle, double x, double y, double z ) +{ + // init matrix + LoadIdentity(); + + // normalize axis of rotation + Vector3 axis( x, y, z ); + if ( !axis.Normalize() ) { + // NULL vector: return identity matrix + return *this; + } + + // calculate unit quaternion corresponding to rotation + double phi2 = -angle / 2; + double sinphi2 = sin( phi2 ); + double W = cos( phi2 ); + double X = sinphi2 * axis.getX(); + double Y = sinphi2 * axis.getY(); + double Z = sinphi2 * axis.getZ(); + + // calculate intermediate terms + double X2 = X * X; + double Y2 = Y * Y; + double Z2 = Z * Z; + double XY = X * Y; + double XZ = X * Z; + double YZ = Y * Z; + double WX = W * X; + double WY = W * Y; + double WZ = W * Z; + + // convert quaternion into rotation matrix + m_matrix[ 0 ][ 0 ] = 1 - ( Y2 + Z2 ) * 2; + m_matrix[ 0 ][ 1 ] = ( XY - WZ ) * 2; + m_matrix[ 0 ][ 2 ] = ( XZ + WY ) * 2; + m_matrix[ 1 ][ 0 ] = ( XY + WZ ) * 2; + m_matrix[ 1 ][ 1 ] = 1 - ( X2 + Z2 ) * 2; + m_matrix[ 1 ][ 2 ] = ( YZ - WX ) * 2; + m_matrix[ 2 ][ 0 ] = ( XZ - WY ) * 2; + m_matrix[ 2 ][ 1 ] = ( YZ + WX ) * 2; + m_matrix[ 2 ][ 2 ] = 1 - ( X2 + Y2 ) * 2; + + return *this; +} + + +// create scale matrix -------------------------------------------------------- +// +Transform3& Transform3::LoadScale( double x, double y, double z ) +{ + LoadIdentity(); + m_matrix[ 0 ][ 0 ] = x; + m_matrix[ 1 ][ 1 ] = y; + m_matrix[ 2 ][ 2 ] = z; + return *this; +} + + +// create translation matrix -------------------------------------------------- +// +Transform3& Transform3::LoadTranslation( double x, double y, double z ) +{ + LoadIdentity(); + m_matrix[ 3 ][ 0 ] = x; + m_matrix[ 3 ][ 1 ] = y; + m_matrix[ 3 ][ 2 ] = z; + return *this; +} + + +// rotate around arbitrary axis ----------------------------------------------- +// +Transform3& Transform3::Rotate( double angle, double x, double y, double z ) +{ + Transform3 temp; + temp.LoadRotation( angle, x, y, z ); + Concat( temp ); + return *this; +} + + +// rotate around arbitrary axis (reversed) ------------------------------------ +// +Transform3& Transform3::RotateR( double angle, double x, double y, double z ) +{ + Transform3 temp; + temp.LoadRotation( angle, x, y, z ); + ConcatR( temp ); + return *this; +} + + +// apply scale factors -------------------------------------------------------- +// +Transform3& Transform3::Scale( double x, double y, double z ) +{ + Transform3 temp; + temp.LoadScale( x, y, z ); + Concat( temp ); + return *this; +} + + +// apply scale factors (reversed) --------------------------------------------- +// +Transform3& Transform3::ScaleR( double x, double y, double z ) +{ + Transform3 temp; + temp.LoadScale( x, y, z ); + ConcatR( temp ); + return *this; +} + + +// apply translation ---------------------------------------------------------- +// +Transform3& Transform3::Translate( double x, double y, double z ) +{ + m_matrix[ 3 ][ 0 ] += x; + m_matrix[ 3 ][ 1 ] += y; + m_matrix[ 3 ][ 2 ] += z; + return *this; +} + + +// apply translation (reversed) ----------------------------------------------- +// +Transform3& Transform3::TranslateR( double x, double y, double z ) +{ + Transform3 temp; + temp.LoadTranslation( x, y, z ); + ConcatR( temp ); + return *this; +} + + +BSPLIB_NAMESPACE_END + +//----------------------------------------------------------------------------- |