diff --git a/Code/OysterMath/LinearMath.h b/Code/OysterMath/LinearMath.h index 4a036e99..c09caf32 100644 --- a/Code/OysterMath/LinearMath.h +++ b/Code/OysterMath/LinearMath.h @@ -161,6 +161,32 @@ namespace LinearAlgebra2D return targetMem = ::LinearAlgebra::Matrix3x3( c,-s, 0, s, c, 0, 0, 0, 1 ); } + template + inline ::LinearAlgebra::Matrix2x2 & InverseRotationMatrix( const ::LinearAlgebra::Matrix2x2 &rotationMatrix ) + { + return rotationMatrix.GetTranspose(); + } + + template + inline ::LinearAlgebra::Matrix3x3 & InverseRotationMatrix( const ::LinearAlgebra::Matrix3x3 &rotationMatrix ) + { + return rotationMatrix.GetTranspose(); + } + + template + inline ::LinearAlgebra::Matrix3x3 OrientationMatrix( const ::LinearAlgebra::Matrix2x2 &rotation, const ::LinearAlgebra::Vector2 &translation ) + { + return ::LinearAlgebra::Matrix3x3( ::LinearAlgebra::Vector3(rotation.v[0], 0), + ::LinearAlgebra::Vector3(rotation.v[1], 0), + ::LinearAlgebra::Vector3(translation, 1) ); + } + + template + inline ::LinearAlgebra::Matrix3x3 OrientationMatrix( const ::LinearAlgebra::Matrix3x3 &rotation, const ::LinearAlgebra::Vector2 &translation ) + { + return ::LinearAlgebra::Matrix3x3( rotation.v[0], rotation.v[1], ::LinearAlgebra::Vector3(translation, 1) ); + } + template inline ::LinearAlgebra::Matrix3x3 & OrientationMatrix( const ScalarType &radian, const ::LinearAlgebra::Vector2 &position, ::LinearAlgebra::Matrix3x3 &targetMem = ::LinearAlgebra::Matrix3x3() ) { @@ -197,6 +223,12 @@ namespace LinearAlgebra2D orientationMatrix.m12, orientationMatrix.m22, -orientationMatrix.v[1].xy.Dot(orientationMatrix.v[2].xy), 0, 0, 1 ); } + + template + inline ::LinearAlgebra::Matrix3x3 ExtractRotationMatrix( const ::LinearAlgebra::Matrix3x3 &orientationMatrix ) + { + return ::LinearAlgebra::Matrix3x3( orientationMatrix.v[0], orientationMatrix.v[1], ::LinearAlgebra::Vector3::standard_unit_z ); + } } namespace LinearAlgebra3D @@ -283,6 +315,33 @@ namespace LinearAlgebra3D return targetMem; } + template + inline ::LinearAlgebra::Matrix3x3 & InverseRotationMatrix( const ::LinearAlgebra::Matrix3x3 &rotationMatrix ) + { + return rotationMatrix.GetTranspose(); + } + + template + inline ::LinearAlgebra::Matrix4x4 & InverseRotationMatrix( const ::LinearAlgebra::Matrix4x4 &rotationMatrix ) + { + return rotationMatrix.GetTranspose(); + } + + template + inline ::LinearAlgebra::Matrix4x4 OrientationMatrix( const ::LinearAlgebra::Matrix3x3 &rotation, const ::LinearAlgebra::Vector3 &translation ) + { + return ::LinearAlgebra::Matrix4x4( ::LinearAlgebra::Vector4(rotation.v[0], 0), + ::LinearAlgebra::Vector4(rotation.v[1], 0), + ::LinearAlgebra::Vector4(rotation.v[2], 0), + ::LinearAlgebra::Vector4(translation, 1) ); + } + + template + inline ::LinearAlgebra::Matrix4x4 OrientationMatrix( const ::LinearAlgebra::Matrix4x4 &rotation, const ::LinearAlgebra::Vector3 &translation ) + { + return ::LinearAlgebra::Matrix4x4( rotation.v[0], rotation.v[1], rotation.v[2], ::LinearAlgebra::Vector4(translation, 1) ); + } + template ::LinearAlgebra::Matrix4x4 & OrientationMatrix( const ::LinearAlgebra::Vector3 &normalizedAxis, const ScalarType &deltaRadian, const ::LinearAlgebra::Vector3 &sumTranslation, ::LinearAlgebra::Matrix4x4 &targetMem = ::LinearAlgebra::Matrix4x4() ) { /** @todo TODO: not tested */ @@ -349,6 +408,12 @@ namespace LinearAlgebra3D return orientationMatrix; } + template + inline ::LinearAlgebra::Matrix4x4 ExtractRotationMatrix( const ::LinearAlgebra::Matrix4x4 &orientationMatrix ) + { + return ::LinearAlgebra::Matrix4x4( orientationMatrix.v[0], orientationMatrix.v[1], orientationMatrix.v[2], ::LinearAlgebra::Vector4::standard_unit_w ); + } + /* Creates an orthographic projection matrix designed for DirectX enviroment. width; of the projection sample volume. height; of the projection sample volume. diff --git a/Code/OysterMath/OysterMath.cpp b/Code/OysterMath/OysterMath.cpp index 84b46b7b..fb2486fc 100644 --- a/Code/OysterMath/OysterMath.cpp +++ b/Code/OysterMath/OysterMath.cpp @@ -32,18 +32,51 @@ namespace Oyster { namespace Math2D Float3x3 & RotationMatrix( const Float &radian, Float3x3 &targetMem ) { return ::LinearAlgebra2D::RotationMatrix( radian, targetMem ); } + + Float2x2 & InverseRotationMatrix( const Float2x2 &rotation, Float2x2 &targetMem ) + { + return targetMem = ::LinearAlgebra2D::InverseRotationMatrix( rotation ); + } + + Float3x3 & InverseRotationMatrix( const Float3x3 &rotation, Float3x3 &targetMem ) + { + return targetMem = ::LinearAlgebra2D::InverseRotationMatrix( rotation ); + } + + Float3x3 & OrientationMatrix( const Float2x2 &rotation, const Float2 &translation, Float3x3 &targetMem ) + { + return targetMem = ::LinearAlgebra2D::OrientationMatrix( rotation, translation ); + } + + Float3x3 & OrientationMatrix( const Float3x3 &rotation, const Float2 &translation, Float3x3 &targetMem ) + { + return targetMem = ::LinearAlgebra2D::OrientationMatrix( rotation, translation ); + } Float3x3 & OrientationMatrix( const Float2 &position, const Float &radian, Float3x3 &targetMem ) - { return ::LinearAlgebra2D::OrientationMatrix( radian, position, targetMem ); } + { + return ::LinearAlgebra2D::OrientationMatrix( radian, position, targetMem ); + } Float3x3 & OrientationMatrix( const Float2 &position, const Float2 &lookAt, Float3x3 &targetMem ) - { return ::LinearAlgebra2D::OrientationMatrix( lookAt, position, targetMem ); } + { + return ::LinearAlgebra2D::OrientationMatrix( lookAt, position, targetMem ); + } Float3x3 & OrientationMatrix( const Float2 &position, Float radian, const Float2 &localCenterOfRotation, Float3x3 &targetMem ) - { return ::LinearAlgebra2D::OrientationMatrix( radian, position, localCenterOfRotation, targetMem ); } + { + return ::LinearAlgebra2D::OrientationMatrix( radian, position, localCenterOfRotation, targetMem ); + } Float3x3 & InverseOrientationMatrix( const Float3x3 &orientationMatrix, Float3x3 &targetMem ) - { return ::LinearAlgebra2D::InverseOrientationMatrix( orientationMatrix, targetMem ); } + { + return ::LinearAlgebra2D::InverseOrientationMatrix( orientationMatrix, targetMem ); + } + + Float3x3 & ExtractRotationMatrix( const Float3x3 &orientation, Float3x3 &targetMem ) + { + return targetMem = ::LinearAlgebra2D::ExtractRotationMatrix( orientation ); + } } } namespace Oyster { namespace Math3D @@ -62,27 +95,66 @@ namespace Oyster { namespace Math3D Float4x4 & RotationMatrix( const Float &radian, const Float3 &normalizedAxis, Float4x4 &targetMem ) { return ::LinearAlgebra3D::RotationMatrix( normalizedAxis, radian, targetMem ); } + + Float3x3 & InverseRotationMatrix( const Float3x3 &rotation, Float3x3 &targetMem ) + { + return targetMem = ::LinearAlgebra3D::InverseRotationMatrix( rotation ); + } + + Float4x4 & InverseRotationMatrix( const Float4x4 &rotation, Float4x4 &targetMem ) + { + return targetMem = ::LinearAlgebra3D::InverseRotationMatrix( rotation ); + } + + Float4x4 & OrientationMatrix( const Float3x3 &rotation, const Float3 &translation, Float4x4 &targetMem ) + { + return targetMem = ::LinearAlgebra3D::OrientationMatrix( rotation, translation ); + } + + Float4x4 & OrientationMatrix( const Float4x4 &rotation, const Float3 &translation, Float4x4 &targetMem ) + { + return targetMem = ::LinearAlgebra3D::OrientationMatrix( rotation, translation ); + } Float4x4 & OrientationMatrix( const Float3 &normalizedAxis, const Float & deltaRadian, const Float3 &sumTranslation, Float4x4 &targetMem ) - { return ::LinearAlgebra3D::OrientationMatrix( normalizedAxis, deltaRadian, sumTranslation, targetMem ); } + { + return ::LinearAlgebra3D::OrientationMatrix( normalizedAxis, deltaRadian, sumTranslation, targetMem ); + } Float4x4 & OrientationMatrix( const Float3 &sumDeltaAngularAxis, const Float3 &sumTranslation, Float4x4 &targetMem ) - { return ::LinearAlgebra3D::OrientationMatrix( sumDeltaAngularAxis, sumTranslation, targetMem ); } + { + return ::LinearAlgebra3D::OrientationMatrix( sumDeltaAngularAxis, sumTranslation, targetMem ); + } Float4x4 & OrientationMatrix( const Float3 &sumDeltaAngularAxis, const Float3 &sumTranslation, const Float3 ¢erOfMass, Float4x4 &targetMem ) - { return ::LinearAlgebra3D::OrientationMatrix( sumDeltaAngularAxis, sumTranslation, centerOfMass, targetMem ); } + { + return ::LinearAlgebra3D::OrientationMatrix( sumDeltaAngularAxis, sumTranslation, centerOfMass, targetMem ); + } Float4x4 & InverseOrientationMatrix( const Float4x4 &orientationMatrix, Float4x4 &targetMem ) - { return ::LinearAlgebra3D::InverseOrientationMatrix( orientationMatrix, targetMem ); } + { + return ::LinearAlgebra3D::InverseOrientationMatrix( orientationMatrix, targetMem ); + } Float4x4 & UpdateOrientationMatrix( const Float3 &deltaPosition, const Float4x4 &deltaRotationMatrix, Float4x4 &orientationMatrix ) - { return ::LinearAlgebra3D::UpdateOrientationMatrix( deltaPosition, deltaRotationMatrix, orientationMatrix ); } + { + return ::LinearAlgebra3D::UpdateOrientationMatrix( deltaPosition, deltaRotationMatrix, orientationMatrix ); + } + + Float4x4 & ExtractRotationMatrix( const Float4x4 &orientation, Float4x4 &targetMem ) + { + return targetMem = ::LinearAlgebra3D::ExtractRotationMatrix( orientation ); + } Float4x4 & ProjectionMatrix_Orthographic( const Float &width, const Float &height, const Float &nearClip, const Float &farClip, Float4x4 &targetMem ) - { return ::LinearAlgebra3D::ProjectionMatrix_Orthographic( width, height, nearClip, farClip, targetMem ); } + { + return ::LinearAlgebra3D::ProjectionMatrix_Orthographic( width, height, nearClip, farClip, targetMem ); + } Float4x4 & ProjectionMatrix_Perspective( const Float &verticalFoV, const Float &aspectRatio, const Float &nearClip, const Float &farClip, Float4x4 &targetMem ) - { return ::LinearAlgebra3D::ProjectionMatrix_Perspective( verticalFoV, aspectRatio, nearClip, farClip, targetMem ); } + { + return ::LinearAlgebra3D::ProjectionMatrix_Perspective( verticalFoV, aspectRatio, nearClip, farClip, targetMem ); + } Float3 VectorProjection( const Float3 &vector, const Float3 &axis ) { diff --git a/Code/OysterMath/OysterMath.h b/Code/OysterMath/OysterMath.h index 25381140..fd116b04 100644 --- a/Code/OysterMath/OysterMath.h +++ b/Code/OysterMath/OysterMath.h @@ -114,6 +114,18 @@ namespace Oyster { namespace Math2D /// Oyster's native math library specialized /// Sets and returns targetMem as a counterclockwise rotationMatrix Float3x3 & RotationMatrix( const Float &radian, Float3x3 &targetMem = Float3x3() ); + /// If rotation is assumed to be by all definitions a rotation matrix. Then this is a much faster inverse method. + Float2x2 & InverseRotationMatrix( const Float2x2 &rotation, Float2x2 &targetMem = Float2x2() ); + + /// If rotation is assumed to be by all definitions a rotation matrix. Then this is a much faster inverse method. + Float3x3 & InverseRotationMatrix( const Float3x3 &rotation, Float3x3 &targetMem = Float3x3() ); + + /// Sets and returns targetMem as an orientation Matrix composed by the rotation matrix and translation vector + Float3x3 & OrientationMatrix( const Float2x2 &rotation, const Float2 &translation, Float3x3 &targetMem = Float3x3() ); + + /// Sets and returns targetMem as an orientation Matrix composed by the rotation matrix and translation vector + Float3x3 & OrientationMatrix( const Float3x3 &rotation, const Float2 &translation, Float3x3 &targetMem = Float3x3() ); + /// Sets and returns targetMem as an orientation Matrix with position as translation and radian rotation Float3x3 & OrientationMatrix( const Float2 &position, const Float &radian, Float3x3 &targetMem = Float3x3() ); @@ -126,6 +138,9 @@ namespace Oyster { namespace Math2D /// Oyster's native math library specialized /// If orientationMatrix is assumed to be by all definitions a rigid orientation matrix aka rigid body matrix. Then this is a much faster inverse method. Float3x3 & InverseOrientationMatrix( const Float3x3 &orientationMatrix, Float3x3 &targetMem = Float3x3() ); + + /// Returns targetmem after writing the rotation data from orientation, into it. + Float3x3 & ExtractRotationMatrix( const Float3x3 &orientation, Float3x3 &targetMem = Float3x3() ); } } namespace Oyster { namespace Math3D /// Oyster's native math library specialized for 3D @@ -148,6 +163,18 @@ namespace Oyster { namespace Math3D /// Oyster's native math library specialized /// Please make sure normalizedAxis is normalized. Float4x4 & RotationMatrix( const Float &radian, const Float3 &normalizedAxis, Float4x4 &targetMem = Float4x4() ); + /// If rotation is assumed to be by all definitions a rotation matrix. Then this is a much faster inverse method. + Float3x3 & InverseRotationMatrix( const Float3x3 &rotation, Float3x3 &targetMem = Float3x3() ); + + /// If rotation is assumed to be by all definitions a rotation matrix. Then this is a much faster inverse method. + Float4x4 & InverseRotationMatrix( const Float4x4 &rotation, Float4x4 &targetMem = Float4x4() ); + + /// Sets and returns targetMem as an orientation Matrix composed by the rotation matrix and translation vector + Float4x4 & OrientationMatrix( const Float3x3 &rotation, const Float3 &translation, Float4x4 &targetMem = Float4x4() ); + + /// Sets and returns targetMem as an orientation Matrix composed by the rotation matrix and translation vector + Float4x4 & OrientationMatrix( const Float4x4 &rotation, const Float3 &translation, Float4x4 &targetMem = Float4x4() ); + /******************************************************************* * Sets and returns targetMem as an orientation Matrix * @param normalizedAxis: The normalized vector parallell with the rotationAxis. @@ -187,6 +214,9 @@ namespace Oyster { namespace Math3D /// Oyster's native math library specialized // O1 = T1 * T0 * R1 * R0 Float4x4 & UpdateOrientationMatrix( const Float3 &deltaPosition, const Float4x4 &deltaRotationMatrix, Float4x4 &orientationMatrix ); + /// Returns targetmem after writing the rotation data from orientation, into it. + Float4x4 & ExtractRotationMatrix( const Float4x4 &orientation, Float4x4 &targetMem = Float4x4() ); + /******************************************************************* * Creates an orthographic projection matrix designed for DirectX enviroment. * @param width; of the projection sample volume. diff --git a/Code/OysterPhysics3D/Box.cpp b/Code/OysterPhysics3D/Box.cpp index ed42ba76..ba97c5c5 100644 --- a/Code/OysterPhysics3D/Box.cpp +++ b/Code/OysterPhysics3D/Box.cpp @@ -8,19 +8,28 @@ using namespace ::Oyster::Collision3D; using namespace ::Oyster::Math3D; -Box::Box( ) : ICollideable(Type_box), orientation(Float4x4::identity), boundingOffset() {} -Box::Box( const Float4x4 &o, const Float3 &s ) : ICollideable(Type_box), orientation(o), boundingOffset(s*0.5) {} +Box::Box( ) + : ICollideable(Type_box), rotation(Float4x4::identity), center(0.0f), boundingOffset(0.5f) +{} + +Box::Box( const Float4x4 &r, const Float3 &p, const Float3 &s ) + : ICollideable(Type_box), rotation(r), center(p), boundingOffset(s*0.5) +{} + Box::~Box( ) {} Box & Box::operator = ( const Box &box ) { - this->orientation = box.orientation; + this->rotation = box.rotation; + this->center = box.center; this->boundingOffset = box.boundingOffset; return *this; } ::Utility::DynamicMemory::UniquePointer Box::Clone( ) const -{ return ::Utility::DynamicMemory::UniquePointer( new Box(*this) ); } +{ + return ::Utility::DynamicMemory::UniquePointer( new Box(*this) ); +} bool Box::Intersects( const ICollideable *target ) const { diff --git a/Code/OysterPhysics3D/Box.h b/Code/OysterPhysics3D/Box.h index 034639d0..ee46d6b6 100644 --- a/Code/OysterPhysics3D/Box.h +++ b/Code/OysterPhysics3D/Box.h @@ -16,19 +16,18 @@ namespace Oyster { namespace Collision3D public: union { - struct{ ::Oyster::Math::Float4x4 orientation; ::Oyster::Math::Float3 boundingOffset; }; + struct{ ::Oyster::Math::Float4x4 rotation; ::Oyster::Math::Float3 center, boundingOffset; }; struct { ::Oyster::Math::Float3 xAxis; ::Oyster::Math::Float paddingA; ::Oyster::Math::Float3 yAxis; ::Oyster::Math::Float paddingB; ::Oyster::Math::Float3 zAxis; ::Oyster::Math::Float paddingC; - ::Oyster::Math::Float3 center; }; - char byte[sizeof(::Oyster::Math::Float4x4) + sizeof(::Oyster::Math::Float3)]; + char byte[sizeof(::Oyster::Math::Float4x4) + 2*sizeof(::Oyster::Math::Float3)]; }; Box( ); - Box( const ::Oyster::Math::Float4x4 &orientation, const ::Oyster::Math::Float3 &size ); + Box( const ::Oyster::Math::Float4x4 &rotation, const ::Oyster::Math::Float3 &worldPos, const ::Oyster::Math::Float3 &size ); virtual ~Box( ); Box & operator = ( const Box &box ); diff --git a/Code/OysterPhysics3D/OysterCollision3D.cpp b/Code/OysterPhysics3D/OysterCollision3D.cpp index 5cb5a1bc..131cad62 100644 --- a/Code/OysterPhysics3D/OysterCollision3D.cpp +++ b/Code/OysterPhysics3D/OysterCollision3D.cpp @@ -6,7 +6,8 @@ #include "Utilities.h" #include -using namespace Oyster::Math3D; +using namespace ::Oyster::Math3D; +using namespace ::Utility::Value; namespace Oyster { namespace Collision3D { namespace Utility { @@ -19,13 +20,13 @@ namespace Oyster { namespace Collision3D { namespace Utility // Float calculations can suffer roundingerrors. Which is where epsilon = 1e-20 comes into the picture inline bool EqualsZero( const Float &value ) { // by Dan Andersson - return ::Utility::Value::Abs( value ) < epsilon; + return Abs( value ) < epsilon; } // Float calculations can suffer roundingerrors. Which is where epsilon = 1e-20 comes into the picture inline bool NotEqualsZero( const Float &value ) { // by Dan Andersson - return ::Utility::Value::Abs( value ) > epsilon; + return Abs( value ) > epsilon; } // returns true if miss/reject @@ -39,8 +40,8 @@ namespace Oyster { namespace Collision3D { namespace Utility t2 = e - boundingOffset; t1 /= f; t2 /= f; if( t1 > t2 ) ::Utility::Element::Swap( t1, t2 ); - tMin = ::Utility::Value::Max( tMin, t1 ); - tMax = ::Utility::Value::Min( tMax, t2 ); + tMin = Max( tMin, t1 ); + tMax = Min( tMax, t2 ); if( tMin > tMax ) return true; if( tMax < 0.0f ) return true; } @@ -65,101 +66,140 @@ namespace Oyster { namespace Collision3D { namespace Utility { // by Dan Andersson Float3 c = (box.maxVertex + box.minVertex) * 0.5f, // box.Center h = (box.maxVertex - box.minVertex) * 0.5f; // box.halfSize - boxExtend = h.x * ::Utility::Value::Abs(plane.normal.x); // Box max extending towards plane - boxExtend += h.y * ::Utility::Value::Abs(plane.normal.y); - boxExtend += h.z * ::Utility::Value::Abs(plane.normal.z); + boxExtend = h.x * Abs(plane.normal.x); // Box max extending towards plane + boxExtend += h.y * Abs(plane.normal.y); + boxExtend += h.z * Abs(plane.normal.z); centerDistance = c.Dot(plane.normal) + plane.phasing; // distance between box center and plane } void Compare( Float &boxExtend, Float ¢erDistance, const Plane &plane, const Box &box ) { // by Dan Andersson - boxExtend = box.boundingOffset.x * ::Utility::Value::Abs(plane.normal.Dot(box.orientation.v[0].xyz)); // Box max extending towards plane - boxExtend += box.boundingOffset.y * ::Utility::Value::Abs(plane.normal.Dot(box.orientation.v[1].xyz)); - boxExtend += box.boundingOffset.z * ::Utility::Value::Abs(plane.normal.Dot(box.orientation.v[2].xyz)); + boxExtend = box.boundingOffset.x * Abs(plane.normal.Dot(box.xAxis)); // Box max extending towards plane + boxExtend += box.boundingOffset.y * Abs(plane.normal.Dot(box.yAxis)); + boxExtend += box.boundingOffset.z * Abs(plane.normal.Dot(box.zAxis)); - centerDistance = box.orientation.v[3].xyz.Dot(plane.normal) + plane.phasing; // distance between box center and plane + centerDistance = box.center.Dot(plane.normal) + plane.phasing; // distance between box center and plane } - bool FifteenAxisVsAlignedAxisOverlappingChecks( const Float3 &boundingOffsetA, const Float3 &boundingOffsetB, const Float4x4 &orientationB ) - { // by Dan Andersson - Float4x4 absOrientationB; - { - Float4x4 tO = orientationB.GetTranspose(); - absOrientationB.v[0] = ::Utility::Value::Abs(tO.v[0]); - if( absOrientationB.v[0].w > boundingOffsetA.x + boundingOffsetB.Dot(absOrientationB.v[0].xyz) ) return false; - absOrientationB.v[1] = ::Utility::Value::Abs(tO.v[1]); - if( absOrientationB.v[1].w > boundingOffsetA.y + boundingOffsetB.Dot(absOrientationB.v[1].xyz) ) return false; - absOrientationB.v[2] = ::Utility::Value::Abs(tO.v[2]); - if( absOrientationB.v[2].w > boundingOffsetA.z + boundingOffsetB.Dot(absOrientationB.v[2].xyz) ) return false; + bool SeperatingAxisTest_AxisAlignedVsTransformedBox( const Float3 &boundingOffsetA, const Float3 &boundingOffsetB, const Float4x4 &rotationB, const Float3 &worldOffset ) + { // by Dan Andersson + + /***************************************************************** + * Uses the Seperating Axis Theorem + * if( |t dot s| > hA dot |s * RA| + hB dot |s * RB| ) then not intersecting + * |t dot s| > hA dot |s| + hB dot |s * RB| .. as RA = I + * + * t: objectB's offset from objectA [worldOffset] + * s: current comparison axis + * hA: boundingReach vector of objectA. Only absolute values is assumed. [boundingOffsetA] + * hB: boundingReach vector of objectB. Only absolute values is assumed. [boundingOffsetB] + * RA: rotation matrix of objectA. Is identity matrix here, thus omitted. + * RB: rotation matrix of objectB. Is transformed into objectA's view at this point. [rotationB] + * + * Note: s * RB = (RB^T * s)^T = (RB^-1 * s)^T .... vector == vector^T + *****************************************************************/ + + Float4x4 absRotationB = Abs(rotationB); + Float3 absWorldOffset = Abs(worldOffset); // |t|: [absWorldOffset] + + // s = { 1, 0, 0 } [ RA.v[0] ] + if( absWorldOffset.x > boundingOffsetA.x + boundingOffsetB.Dot(Float3(absRotationB.v[0].x, absRotationB.v[1].x, absRotationB.v[2].x)) ) + { // |t dot s| > hA dot |s| + hB dot |s * RB| -->> t.x > hA.x + hB dot |{RB.v[0].x, RB.v[1].x, RB.v[2].x}| + return false; } - absOrientationB.Transpose(); - if( ::Utility::Value::Abs(orientationB.v[3].Dot(orientationB.v[0])) > boundingOffsetB.x + boundingOffsetA.Dot(absOrientationB.v[0].xyz) ) return false; - if( ::Utility::Value::Abs(orientationB.v[3].Dot(orientationB.v[1])) > boundingOffsetB.x + boundingOffsetA.Dot(absOrientationB.v[1].xyz) ) return false; - if( ::Utility::Value::Abs(orientationB.v[3].Dot(orientationB.v[2])) > boundingOffsetB.x + boundingOffsetA.Dot(absOrientationB.v[2].xyz) ) return false; + // s = { 0, 1, 0 } [ RA.v[1] ] + if( absWorldOffset.y > boundingOffsetA.y + boundingOffsetB.Dot(Float3(absRotationB.v[0].y, absRotationB.v[1].y, absRotationB.v[2].y)) ) + { // t.y > hA.y + hB dot |{RB.v[0].y, RB.v[1].y, RB.v[2].y}| + return false; + } + + // s = { 0, 0, 1 } [ RA.v[2] ] + if( absWorldOffset.z > boundingOffsetA.z + boundingOffsetB.Dot(Float3(absRotationB.v[0].z, absRotationB.v[1].z, absRotationB.v[2].z)) ) + { // t.z > hA.z + hB dot |{RB.v[0].z, RB.v[1].z, RB.v[2].z}| + return false; + } + + // s = RB.v[0].xyz + if( Abs(worldOffset.Dot(rotationB.v[0].xyz)) > boundingOffsetA.Dot(absRotationB.v[0].xyz) + boundingOffsetB.x ) + { // |t dot s| > hA dot |s| + hB dot |s * RB| -->> |t dot s| > hA dot |s| + hB dot |{1, 0, 0}| -->> |t dot s| > hA dot |s| + hB.x + return false; + } + + // s = RB.v[1].xyz + if( Abs(worldOffset.Dot(rotationB.v[1].xyz)) > boundingOffsetA.Dot(absRotationB.v[1].xyz) + boundingOffsetB.y ) + { // |t dot s| > hA dot |s| + hB.y + return false; + } + + // s = RB.v[2].xyz + if( Abs(worldOffset.Dot(rotationB.v[2].xyz)) > boundingOffsetA.Dot(absRotationB.v[2].xyz) + boundingOffsetB.z ) + { // |t dot s| > hA dot |s| + hB.z + return false; + } + + // s = ( 1,0,0 ) x rotationB.v[0].xyz: + Float d = boundingOffsetA.y * absRotationB.v[0].z; + d += boundingOffsetA.z * absRotationB.v[0].y; + d += boundingOffsetB.y * absRotationB.v[2].x; + d += boundingOffsetB.z * absRotationB.v[1].x; + if( Abs(worldOffset.z*rotationB.v[0].y - worldOffset.y*rotationB.v[0].z) > d ) return false; + + // s = ( 1,0,0 ) x rotationB.v[1].xyz: + d = boundingOffsetA.y * absRotationB.v[1].z; + d += boundingOffsetA.z * absRotationB.v[1].y; + d += boundingOffsetB.x * absRotationB.v[2].x; + d += boundingOffsetB.z * absRotationB.v[0].x; + if( Abs(worldOffset.z*rotationB.v[1].y - worldOffset.y*rotationB.v[1].z) > d ) return false; + + // s = ( 1,0,0 ) x rotationB.v[2].xyz: + d = boundingOffsetA.y * absRotationB.v[2].z; + d += boundingOffsetA.z * absRotationB.v[2].y; + d += boundingOffsetB.x * absRotationB.v[1].x; + d += boundingOffsetB.y * absRotationB.v[0].x; + if( Abs(worldOffset.z*rotationB.v[2].y - worldOffset.y*rotationB.v[2].z) > d ) return false; + + // s = ( 0,1,0 ) x rotationB.v[0].xyz: + d = boundingOffsetA.x * absRotationB.v[0].z; + d += boundingOffsetA.z * absRotationB.v[0].x; + d += boundingOffsetB.y * absRotationB.v[2].y; + d += boundingOffsetB.z * absRotationB.v[1].y; + if( Abs(worldOffset.x*rotationB.v[0].z - worldOffset.z*rotationB.v[0].x) > d ) return false; - // ( 1,0,0 ) x orientationB.v[0].xyz: - Float d = boundingOffsetA.y * absOrientationB.v[0].z; - d += boundingOffsetA.z * absOrientationB.v[0].y; - d += boundingOffsetB.y * absOrientationB.v[2].x; - d += boundingOffsetB.z * absOrientationB.v[1].x; - if( ::Utility::Value::Abs(orientationB.v[3].z*orientationB.v[0].y - orientationB.v[3].y*orientationB.v[0].z) > d ) return false; + // s = ( 0,1,0 ) x rotationB.v[1].xyz: + d = boundingOffsetA.x * absRotationB.v[1].z; + d += boundingOffsetA.z * absRotationB.v[1].x; + d += boundingOffsetB.x * absRotationB.v[2].y; + d += boundingOffsetB.z * absRotationB.v[0].y; + if( Abs(worldOffset.x*rotationB.v[1].z - worldOffset.z*rotationB.v[1].x) > d ) return false; - // ( 1,0,0 ) x orientationB.v[1].xyz: - d = boundingOffsetA.y * absOrientationB.v[1].z; - d += boundingOffsetA.z * absOrientationB.v[1].y; - d += boundingOffsetB.x * absOrientationB.v[2].x; - d += boundingOffsetB.z * absOrientationB.v[0].x; - if( ::Utility::Value::Abs(orientationB.v[3].z*orientationB.v[1].y - orientationB.v[3].y*orientationB.v[1].z) > d ) return false; + // s = ( 0,1,0 ) x rotationB.v[2].xyz: + d = boundingOffsetA.x * absRotationB.v[2].z; + d += boundingOffsetA.z * absRotationB.v[2].x; + d += boundingOffsetB.x * absRotationB.v[1].y; + d += boundingOffsetB.y * absRotationB.v[0].y; + if( Abs(worldOffset.x*rotationB.v[2].z - worldOffset.z*rotationB.v[2].x) > d ) return false; - // ( 1,0,0 ) x orientationB.v[2].xyz: - d = boundingOffsetA.y * absOrientationB.v[2].z; - d += boundingOffsetA.z * absOrientationB.v[2].y; - d += boundingOffsetB.x * absOrientationB.v[1].x; - d += boundingOffsetB.y * absOrientationB.v[0].x; - if( ::Utility::Value::Abs(orientationB.v[3].z*orientationB.v[2].y - orientationB.v[3].y*orientationB.v[2].z) > d ) return false; + // s = ( 0,0,1 ) x rotationB.v[0].xyz: + d = boundingOffsetA.x * absRotationB.v[0].y; + d += boundingOffsetA.y * absRotationB.v[0].x; + d += boundingOffsetB.y * absRotationB.v[2].z; + d += boundingOffsetB.z * absRotationB.v[1].z; + if( Abs(worldOffset.y*rotationB.v[0].x - worldOffset.x*rotationB.v[0].y) > d ) return false; - // ( 0,1,0 ) x orientationB.v[0].xyz: - d = boundingOffsetA.x * absOrientationB.v[0].z; - d += boundingOffsetA.z * absOrientationB.v[0].x; - d += boundingOffsetB.y * absOrientationB.v[2].y; - d += boundingOffsetB.z * absOrientationB.v[1].y; - if( ::Utility::Value::Abs(orientationB.v[3].x*orientationB.v[0].z - orientationB.v[3].z*orientationB.v[0].x) > d ) return false; - - // ( 0,1,0 ) x orientationB.v[1].xyz: - d = boundingOffsetA.x * absOrientationB.v[1].z; - d += boundingOffsetA.z * absOrientationB.v[1].x; - d += boundingOffsetB.x * absOrientationB.v[2].y; - d += boundingOffsetB.z * absOrientationB.v[0].y; - if( ::Utility::Value::Abs(orientationB.v[3].x*orientationB.v[1].z - orientationB.v[3].z*orientationB.v[1].x) > d ) return false; + // s = ( 0,0,1 ) x rotationB.v[1].xyz: + d = boundingOffsetA.x * absRotationB.v[1].y; + d += boundingOffsetA.y * absRotationB.v[1].x; + d += boundingOffsetB.x * absRotationB.v[2].z; + d += boundingOffsetB.z * absRotationB.v[0].z; + if( Abs(worldOffset.y*rotationB.v[1].x - worldOffset.x*rotationB.v[1].y) > d ) return false; - // ( 0,1,0 ) x orientationB.v[2].xyz: - d = boundingOffsetA.x * absOrientationB.v[2].z; - d += boundingOffsetA.z * absOrientationB.v[2].x; - d += boundingOffsetB.x * absOrientationB.v[1].y; - d += boundingOffsetB.y * absOrientationB.v[0].y; - if( ::Utility::Value::Abs(orientationB.v[3].x*orientationB.v[2].z - orientationB.v[3].z*orientationB.v[2].x) > d ) return false; - - // ( 0,0,1 ) x orientationB.v[0].xyz: - d = boundingOffsetA.x * absOrientationB.v[0].y; - d += boundingOffsetA.y * absOrientationB.v[0].x; - d += boundingOffsetB.y * absOrientationB.v[2].z; - d += boundingOffsetB.z * absOrientationB.v[1].z; - if( ::Utility::Value::Abs(orientationB.v[3].y*orientationB.v[0].x - orientationB.v[3].x*orientationB.v[0].y) > d ) return false; - - // ( 0,0,1 ) x orientationB.v[1].xyz: - d = boundingOffsetA.x * absOrientationB.v[1].y; - d += boundingOffsetA.y * absOrientationB.v[1].x; - d += boundingOffsetB.x * absOrientationB.v[2].z; - d += boundingOffsetB.z * absOrientationB.v[0].z; - if( ::Utility::Value::Abs(orientationB.v[3].y*orientationB.v[1].x - orientationB.v[3].x*orientationB.v[1].y) > d ) return false; - - // ( 0,0,1 ) x orientationB.v[2].xyz: - d = boundingOffsetA.x * absOrientationB.v[2].y; - d += boundingOffsetA.y * absOrientationB.v[2].x; - d += boundingOffsetB.x * absOrientationB.v[1].z; - d += boundingOffsetB.y * absOrientationB.v[0].z; - if( ::Utility::Value::Abs(orientationB.v[3].y*orientationB.v[2].x - orientationB.v[3].x*orientationB.v[2].y) > d ) return false; + // s = ( 0,0,1 ) x rotationB.v[2].xyz: + d = boundingOffsetA.x * absRotationB.v[2].y; + d += boundingOffsetA.y * absRotationB.v[2].x; + d += boundingOffsetB.x * absRotationB.v[1].z; + d += boundingOffsetB.y * absRotationB.v[0].z; + if( Abs(worldOffset.y*rotationB.v[2].x - worldOffset.x*rotationB.v[2].y) > d ) return false; return true; } @@ -353,8 +393,8 @@ namespace Oyster { namespace Collision3D { namespace Utility bool Intersect( const BoxAxisAligned &box, const Sphere &sphere ) { // by Dan Andersson - Float3 e = ::Utility::Value::Max( box.minVertex - sphere.center, Float3::null ); - e += ::Utility::Value::Max( sphere.center - box.maxVertex, Float3::null ); + Float3 e = Max( box.minVertex - sphere.center, Float3::null ); + e += Max( sphere.center - box.maxVertex, Float3::null ); if( e.Dot(e) > (sphere.radius * sphere.radius) ) return false; return true; @@ -385,17 +425,17 @@ namespace Oyster { namespace Collision3D { namespace Utility bool Intersect( const Box &box, const Point &point ) { // by Dan Andersson - Float3 dPos = point.center - box.orientation.v[3].xyz; + Float3 dPos = point.center - box.center; - Float coordinate = dPos.Dot( box.orientation.v[0].xyz ); + Float coordinate = dPos.Dot( box.xAxis ); if( coordinate > box.boundingOffset.x ) return false; if( coordinate < -box.boundingOffset.x ) return false; - coordinate = dPos.Dot( box.orientation.v[1].xyz ); + coordinate = dPos.Dot( box.yAxis ); if( coordinate > box.boundingOffset.y ) return false; if( coordinate < -box.boundingOffset.y ) return false; - coordinate = dPos.Dot( box.orientation.v[2].xyz ); + coordinate = dPos.Dot( box.zAxis ); if( coordinate > box.boundingOffset.z ) return false; if( coordinate < -box.boundingOffset.z ) return false; @@ -419,11 +459,11 @@ namespace Oyster { namespace Collision3D { namespace Utility bool Intersect( const Box &box, const Sphere &sphere ) { // by Dan Andersson - Float3 e = sphere.center - box.orientation.v[3].xyz, - centerL = Float3( e.Dot(box.orientation.v[0].xyz), e.Dot(box.orientation.v[1].xyz), e.Dot(box.orientation.v[2].xyz) ); + Float3 e = sphere.center - box.center, + centerL = Float3( e.Dot(box.xAxis), e.Dot(box.yAxis), e.Dot(box.zAxis) ); - e = ::Utility::Value::Max( (box.boundingOffset + centerL)*=-1.0f, Float3::null ); - e += ::Utility::Value::Max( centerL - box.boundingOffset, Float3::null ); + e = Max( (box.boundingOffset + centerL)*=-1.0f, Float3::null ); + e += Max( centerL - box.boundingOffset, Float3::null ); if( e.Dot(e) > (sphere.radius * sphere.radius) ) return false; return true; @@ -440,20 +480,20 @@ namespace Oyster { namespace Collision3D { namespace Utility bool Intersect( const Box &boxA, const BoxAxisAligned &boxB ) { // by Dan Andersson - Float3 alignedOffsetBoundaries = boxB.maxVertex - boxB.minVertex; - Float4x4 translated = boxA.orientation; - translated.v[3].xyz -= boxB.minVertex; - translated.v[3].xyz += alignedOffsetBoundaries * 0.5f; - alignedOffsetBoundaries = ::Utility::Value::Abs(alignedOffsetBoundaries); - return Private::FifteenAxisVsAlignedAxisOverlappingChecks( alignedOffsetBoundaries, boxA.boundingOffset, translated ); + Float3 alignedOffsetBoundaries = (boxB.maxVertex - boxB.minVertex) * 0.5f, + offset = boxA.center - Average( boxB.minVertex, boxB.maxVertex ); + return Private::SeperatingAxisTest_AxisAlignedVsTransformedBox( alignedOffsetBoundaries, boxA.boundingOffset, boxA.rotation, offset ); } bool Intersect( const Box &boxA, const Box &boxB ) { // by Dan Andersson - Float4x4 M; - InverseOrientationMatrix( boxA.orientation, M ); - TransformMatrix( M, boxB.orientation, M ); /// TODO: not verified - return Private::FifteenAxisVsAlignedAxisOverlappingChecks( boxA.boundingOffset, boxB.boundingOffset, M ); + Float4x4 orientationA = OrientationMatrix(boxA.rotation, boxA.center), + orientationB = OrientationMatrix(boxB.rotation, boxB.center), + invOrientationA = InverseOrientationMatrix( orientationA ); + + orientationB = TransformMatrix( invOrientationA, orientationB ); + + return Private::SeperatingAxisTest_AxisAlignedVsTransformedBox( boxA.boundingOffset, boxB.boundingOffset, ExtractRotationMatrix(orientationB), orientationB.v[3].xyz ); } bool Intersect( const Frustrum &frustrum, const Point &point ) @@ -490,37 +530,37 @@ namespace Oyster { namespace Collision3D { namespace Utility if( Intersect(frustrum.leftPlane, ray, distance) ) { intersected = true; - connectDistance = ::Utility::Value::Min( connectDistance, distance ); + connectDistance = Min( connectDistance, distance ); } if( Intersect(frustrum.rightPlane, ray, distance) ) { intersected = true; - connectDistance = ::Utility::Value::Min( connectDistance, distance ); + connectDistance = Min( connectDistance, distance ); } if( Intersect(frustrum.bottomPlane, ray, distance) ) { intersected = true; - connectDistance = ::Utility::Value::Min( connectDistance, distance ); + connectDistance = Min( connectDistance, distance ); } if( Intersect(frustrum.topPlane, ray, distance) ) { intersected = true; - connectDistance = ::Utility::Value::Min( connectDistance, distance ); + connectDistance = Min( connectDistance, distance ); } if( Intersect(frustrum.nearPlane, ray, distance) ) { intersected = true; - connectDistance = ::Utility::Value::Min( connectDistance, distance ); + connectDistance = Min( connectDistance, distance ); } if( Intersect(frustrum.farPlane, ray, distance) ) { intersected = true; - connectDistance = ::Utility::Value::Min( connectDistance, distance ); + connectDistance = Min( connectDistance, distance ); } if( intersected ) return true; diff --git a/Code/OysterPhysics3D/RigidBody.cpp b/Code/OysterPhysics3D/RigidBody.cpp index 319d4da4..9f031979 100644 --- a/Code/OysterPhysics3D/RigidBody.cpp +++ b/Code/OysterPhysics3D/RigidBody.cpp @@ -40,7 +40,7 @@ void RigidBody::Update_LeapFrog( Float deltaTime ) { // by Dan Andersson: Euler leap frog update when Runga Kutta is not needed // Important! The member data is all world data except the Inertia tensor. Thus a new InertiaTensor needs to be created to be compatible with the rest of the world data. - Float4x4 wMomentOfInertiaTensor = TransformMatrix( this->box.orientation, this->momentOfInertiaTensor ); + Float4x4 wMomentOfInertiaTensor = TransformMatrix( this->box.rotation, this->momentOfInertiaTensor ); // updating the linear // dv = dt * a = dt * F / m @@ -63,10 +63,9 @@ void RigidBody::Update_LeapFrog( Float deltaTime ) deltaRadian = ::std::sqrt( deltaRadian ); rotationAxis /= deltaRadian; - // using rotationAxis, deltaRadian and deltaPos to create a matrix to update the orientation matrix - UpdateOrientationMatrix( deltaPos, RotationMatrix(deltaRadian, rotationAxis), this->box.orientation ); - - /** @todo TODO: ISSUE! how is momentOfInertiaTensor related to the orientation of the RigidBody? */ + // using rotationAxis, deltaRadian and deltaPos to create a matrix to update the box + this->box.center += deltaPos; + TransformMatrix( RotationMatrix(deltaRadian, rotationAxis), this->box.rotation, this->box.rotation ); } else { // no rotation, only use deltaPos to translate the RigidBody diff --git a/Code/OysterPhysics3D/RigidBody.h b/Code/OysterPhysics3D/RigidBody.h index 4184ca2e..4b7f65c0 100644 --- a/Code/OysterPhysics3D/RigidBody.h +++ b/Code/OysterPhysics3D/RigidBody.h @@ -34,8 +34,6 @@ namespace Oyster { namespace Physics3D // ACCESS METHODS ///////////////////////////// - ::Oyster::Math::Float4x4 & AccessOrientation(); - const ::Oyster::Math::Float4x4 & AccessOrientation() const; ::Oyster::Math::Float3 & AccessBoundingReach(); const ::Oyster::Math::Float3 & AccessBoundingReach() const; ::Oyster::Math::Float3 & AccessCenter(); @@ -46,7 +44,7 @@ namespace Oyster { namespace Physics3D const ::Oyster::Math::Float4x4 & GetMomentOfInertia() const; const ::Oyster::Math::Float & GetMass() const; - const ::Oyster::Math::Float4x4 & GetOrientation() const; + const ::Oyster::Math::Float4x4 GetOrientation() const; ::Oyster::Math::Float4x4 GetView() const; const ::Oyster::Math::Float4x4 & GetToWorldMatrix() const; ::Oyster::Math::Float4x4 GetToLocalMatrix() const;