gmueller/bluecore
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

initial commit

Browse Source
This commit is contained in:
cirdan
2008-01-16 11:45:17 +00:00
commit 8f17a3a819
1068 changed files with 384278 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

405
ode/OPCODE/Ice/IceAABB.cpp Normal file
View File

@ -0,0 +1,405 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains AABB-related code.
* \file IceAABB.cpp
* \author Pierre Terdiman
* \date January, 29, 2000
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* AABB class.
* \class AABB
* \author Pierre Terdiman
* \version 1.0
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Precompiled Header
#include "Stdafx.h"
using namespace IceMaths;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the sum of two AABBs.
* \param aabb [in] the other AABB
* \return Self-Reference
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
AABB& AABB::Add(const AABB& aabb)
{
// Compute new min & max values
Point Min; GetMin(Min);
Point Tmp; aabb.GetMin(Tmp);
Min.Min(Tmp);
Point Max; GetMax(Max);
aabb.GetMax(Tmp);
Max.Max(Tmp);
// Update this
SetMinMax(Min, Max);
return *this;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Makes a cube from the AABB.
* \param cube [out] the cube AABB
* \return cube edge length
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
float AABB::MakeCube(AABB& cube) const
{
Point Ext; GetExtents(Ext);
float Max = Ext.Max();
Point Cnt; GetCenter(Cnt);
cube.SetCenterExtents(Cnt, Point(Max, Max, Max));
return Max;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Makes a sphere from the AABB.
* \param sphere [out] sphere containing the AABB
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void AABB::MakeSphere(Sphere& sphere) const
{
GetExtents(sphere.mCenter);
sphere.mRadius = sphere.mCenter.Magnitude() * 1.00001f; // To make sure sphere::Contains(*this) succeeds
GetCenter(sphere.mCenter);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Checks a box is inside another box.
* \param box [in] the other AABB
* \return true if current box is inside input box
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool AABB::IsInside(const AABB& box) const
{
if(box.GetMin(0)>GetMin(0)) return false;
if(box.GetMin(1)>GetMin(1)) return false;
if(box.GetMin(2)>GetMin(2)) return false;
if(box.GetMax(0)<GetMax(0)) return false;
if(box.GetMax(1)<GetMax(1)) return false;
if(box.GetMax(2)<GetMax(2)) return false;
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the AABB planes.
* \param planes [out] 6 planes surrounding the box
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool AABB::ComputePlanes(Plane* planes) const
{
// Checkings
if(!planes) return false;
Point Center, Extents;
GetCenter(Center);
GetExtents(Extents);
// Writes normals
planes[0].n = Point(1.0f, 0.0f, 0.0f);
planes[1].n = Point(-1.0f, 0.0f, 0.0f);
planes[2].n = Point(0.0f, 1.0f, 0.0f);
planes[3].n = Point(0.0f, -1.0f, 0.0f);
planes[4].n = Point(0.0f, 0.0f, 1.0f);
planes[5].n = Point(0.0f, 0.0f, -1.0f);
// Compute a point on each plane
Point p0 = Point(Center.x+Extents.x, Center.y, Center.z);
Point p1 = Point(Center.x-Extents.x, Center.y, Center.z);
Point p2 = Point(Center.x, Center.y+Extents.y, Center.z);
Point p3 = Point(Center.x, Center.y-Extents.y, Center.z);
Point p4 = Point(Center.x, Center.y, Center.z+Extents.z);
Point p5 = Point(Center.x, Center.y, Center.z-Extents.z);
// Compute d
planes[0].d = -(planes[0].n|p0);
planes[1].d = -(planes[1].n|p1);
planes[2].d = -(planes[2].n|p2);
planes[3].d = -(planes[3].n|p3);
planes[4].d = -(planes[4].n|p4);
planes[5].d = -(planes[5].n|p5);
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the aabb points.
* \param pts [out] 8 box points
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool AABB::ComputePoints(Point* pts) const
{
// Checkings
if(!pts) return false;
// Get box corners
Point min; GetMin(min);
Point max; GetMax(max);
// 7+------+6 0 = ---
// /| /| 1 = +--
// / | / | 2 = ++-
// / 4+---/--+5 3 = -+-
// 3+------+2 / y z 4 = --+
// | / | / | / 5 = +-+
// |/ |/ |/ 6 = +++
// 0+------+1 *---x 7 = -++
// Generate 8 corners of the bbox
pts[0] = Point(min.x, min.y, min.z);
pts[1] = Point(max.x, min.y, min.z);
pts[2] = Point(max.x, max.y, min.z);
pts[3] = Point(min.x, max.y, min.z);
pts[4] = Point(min.x, min.y, max.z);
pts[5] = Point(max.x, min.y, max.z);
pts[6] = Point(max.x, max.y, max.z);
pts[7] = Point(min.x, max.y, max.z);
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Gets vertex normals.
* \param pts [out] 8 box points
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
const Point* AABB::GetVertexNormals() const
{
static float VertexNormals[] =
{
-INVSQRT3, -INVSQRT3, -INVSQRT3,
INVSQRT3, -INVSQRT3, -INVSQRT3,
INVSQRT3, INVSQRT3, -INVSQRT3,
-INVSQRT3, INVSQRT3, -INVSQRT3,
-INVSQRT3, -INVSQRT3, INVSQRT3,
INVSQRT3, -INVSQRT3, INVSQRT3,
INVSQRT3, INVSQRT3, INVSQRT3,
-INVSQRT3, INVSQRT3, INVSQRT3
};
return (const Point*)VertexNormals;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Returns edges.
* \return 24 indices (12 edges) indexing the list returned by ComputePoints()
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
const udword* AABB::GetEdges() const
{
static udword Indices[] = {
0, 1, 1, 2, 2, 3, 3, 0,
7, 6, 6, 5, 5, 4, 4, 7,
1, 5, 6, 2,
3, 7, 4, 0
};
return Indices;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Returns edge normals.
* \return edge normals in local space
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
const Point* AABB::GetEdgeNormals() const
{
static float EdgeNormals[] =
{
0, -INVSQRT2, -INVSQRT2, // 0-1
INVSQRT2, 0, -INVSQRT2, // 1-2
0, INVSQRT2, -INVSQRT2, // 2-3
-INVSQRT2, 0, -INVSQRT2, // 3-0
0, INVSQRT2, INVSQRT2, // 7-6
INVSQRT2, 0, INVSQRT2, // 6-5
0, -INVSQRT2, INVSQRT2, // 5-4
-INVSQRT2, 0, INVSQRT2, // 4-7
INVSQRT2, -INVSQRT2, 0, // 1-5
INVSQRT2, INVSQRT2, 0, // 6-2
-INVSQRT2, INVSQRT2, 0, // 3-7
-INVSQRT2, -INVSQRT2, 0 // 4-0
};
return (const Point*)EdgeNormals;
}
// ===========================================================================
// (C) 1996-98 Vienna University of Technology
// ===========================================================================
// NAME: bboxarea
// TYPE: c++ code
// PROJECT: Bounding Box Area
// CONTENT: Computes area of 2D projection of 3D oriented bounding box
// VERSION: 1.0
// ===========================================================================
// AUTHORS: ds Dieter Schmalstieg
// ep Erik Pojar
// ===========================================================================
// HISTORY:
//
// 19-sep-99 15:23:03 ds last modification
// 01-dec-98 15:23:03 ep created
// ===========================================================================
//----------------------------------------------------------------------------
// SAMPLE CODE STARTS HERE
//----------------------------------------------------------------------------
// NOTE: This sample program requires OPEN INVENTOR!
//indexlist: this table stores the 64 possible cases of classification of
//the eyepoint with respect to the 6 defining planes of the bbox (2^6=64)
//only 26 (3^3-1, where 1 is "inside" cube) of these cases are valid.
//the first 6 numbers in each row are the indices of the bbox vertices that
//form the outline of which we want to compute the area (counterclockwise
//ordering), the 7th entry means the number of vertices in the outline.
//there are 6 cases with a single face and and a 4-vertex outline, and
//20 cases with 2 or 3 faces and a 6-vertex outline. a value of 0 indicates
//an invalid case.
// Original list was made of 7 items, I added an 8th element:
// - to padd on a cache line
// - to repeat the first entry to avoid modulos
//
// I also replaced original ints with sbytes.
static const sbyte gIndexList[64][8] =
{
{-1,-1,-1,-1,-1,-1,-1, 0}, // 0 inside
{ 0, 4, 7, 3, 0,-1,-1, 4}, // 1 left
{ 1, 2, 6, 5, 1,-1,-1, 4}, // 2 right
{-1,-1,-1,-1,-1,-1,-1, 0}, // 3 -
{ 0, 1, 5, 4, 0,-1,-1, 4}, // 4 bottom
{ 0, 1, 5, 4, 7, 3, 0, 6}, // 5 bottom, left
{ 0, 1, 2, 6, 5, 4, 0, 6}, // 6 bottom, right
{-1,-1,-1,-1,-1,-1,-1, 0}, // 7 -
{ 2, 3, 7, 6, 2,-1,-1, 4}, // 8 top
{ 0, 4, 7, 6, 2, 3, 0, 6}, // 9 top, left
{ 1, 2, 3, 7, 6, 5, 1, 6}, //10 top, right
{-1,-1,-1,-1,-1,-1,-1, 0}, //11 -
{-1,-1,-1,-1,-1,-1,-1, 0}, //12 -
{-1,-1,-1,-1,-1,-1,-1, 0}, //13 -
{-1,-1,-1,-1,-1,-1,-1, 0}, //14 -
{-1,-1,-1,-1,-1,-1,-1, 0}, //15 -
{ 0, 3, 2, 1, 0,-1,-1, 4}, //16 front
{ 0, 4, 7, 3, 2, 1, 0, 6}, //17 front, left
{ 0, 3, 2, 6, 5, 1, 0, 6}, //18 front, right
{-1,-1,-1,-1,-1,-1,-1, 0}, //19 -
{ 0, 3, 2, 1, 5, 4, 0, 6}, //20 front, bottom
{ 1, 5, 4, 7, 3, 2, 1, 6}, //21 front, bottom, left
{ 0, 3, 2, 6, 5, 4, 0, 6}, //22 front, bottom, right
{-1,-1,-1,-1,-1,-1,-1, 0}, //23 -
{ 0, 3, 7, 6, 2, 1, 0, 6}, //24 front, top
{ 0, 4, 7, 6, 2, 1, 0, 6}, //25 front, top, left
{ 0, 3, 7, 6, 5, 1, 0, 6}, //26 front, top, right
{-1,-1,-1,-1,-1,-1,-1, 0}, //27 -
{-1,-1,-1,-1,-1,-1,-1, 0}, //28 -
{-1,-1,-1,-1,-1,-1,-1, 0}, //29 -
{-1,-1,-1,-1,-1,-1,-1, 0}, //30 -
{-1,-1,-1,-1,-1,-1,-1, 0}, //31 -
{ 4, 5, 6, 7, 4,-1,-1, 4}, //32 back
{ 0, 4, 5, 6, 7, 3, 0, 6}, //33 back, left
{ 1, 2, 6, 7, 4, 5, 1, 6}, //34 back, right
{-1,-1,-1,-1,-1,-1,-1, 0}, //35 -
{ 0, 1, 5, 6, 7, 4, 0, 6}, //36 back, bottom
{ 0, 1, 5, 6, 7, 3, 0, 6}, //37 back, bottom, left
{ 0, 1, 2, 6, 7, 4, 0, 6}, //38 back, bottom, right
{-1,-1,-1,-1,-1,-1,-1, 0}, //39 -
{ 2, 3, 7, 4, 5, 6, 2, 6}, //40 back, top
{ 0, 4, 5, 6, 2, 3, 0, 6}, //41 back, top, left
{ 1, 2, 3, 7, 4, 5, 1, 6}, //42 back, top, right
{-1,-1,-1,-1,-1,-1,-1, 0}, //43 invalid
{-1,-1,-1,-1,-1,-1,-1, 0}, //44 invalid
{-1,-1,-1,-1,-1,-1,-1, 0}, //45 invalid
{-1,-1,-1,-1,-1,-1,-1, 0}, //46 invalid
{-1,-1,-1,-1,-1,-1,-1, 0}, //47 invalid
{-1,-1,-1,-1,-1,-1,-1, 0}, //48 invalid
{-1,-1,-1,-1,-1,-1,-1, 0}, //49 invalid
{-1,-1,-1,-1,-1,-1,-1, 0}, //50 invalid
{-1,-1,-1,-1,-1,-1,-1, 0}, //51 invalid
{-1,-1,-1,-1,-1,-1,-1, 0}, //52 invalid
{-1,-1,-1,-1,-1,-1,-1, 0}, //53 invalid
{-1,-1,-1,-1,-1,-1,-1, 0}, //54 invalid
{-1,-1,-1,-1,-1,-1,-1, 0}, //55 invalid
{-1,-1,-1,-1,-1,-1,-1, 0}, //56 invalid
{-1,-1,-1,-1,-1,-1,-1, 0}, //57 invalid
{-1,-1,-1,-1,-1,-1,-1, 0}, //58 invalid
{-1,-1,-1,-1,-1,-1,-1, 0}, //59 invalid
{-1,-1,-1,-1,-1,-1,-1, 0}, //60 invalid
{-1,-1,-1,-1,-1,-1,-1, 0}, //61 invalid
{-1,-1,-1,-1,-1,-1,-1, 0}, //62 invalid
{-1,-1,-1,-1,-1,-1,-1, 0} //63 invalid
};
const sbyte* AABB::ComputeOutline(const Point& local_eye, sdword& num) const
{
// Get box corners
Point min; GetMin(min);
Point max; GetMax(max);
// Compute 6-bit code to classify eye with respect to the 6 defining planes of the bbox
int pos = ((local_eye.x < min.x) ? 1 : 0) // 1 = left
+ ((local_eye.x > max.x) ? 2 : 0) // 2 = right
+ ((local_eye.y < min.y) ? 4 : 0) // 4 = bottom
+ ((local_eye.y > max.y) ? 8 : 0) // 8 = top
+ ((local_eye.z < min.z) ? 16 : 0) // 16 = front
+ ((local_eye.z > max.z) ? 32 : 0); // 32 = back
// Look up number of vertices in outline
num = (sdword)gIndexList[pos][7];
// Zero indicates invalid case
if(!num) return null;
return &gIndexList[pos][0];
}
// calculateBoxArea: computes the screen-projected 2D area of an oriented 3D bounding box
//const Point& eye, //eye point (in bbox object coordinates)
//const AABB& box, //3d bbox
//const Matrix4x4& mat, //free transformation for bbox
//float width, float height, int& num)
float AABB::ComputeBoxArea(const Point& eye, const Matrix4x4& mat, float width, float height, sdword& num) const
{
const sbyte* Outline = ComputeOutline(eye, num);
if(!Outline) return -1.0f;
// Compute box vertices
Point vertexBox[8], dst[8];
ComputePoints(vertexBox);
// Transform all outline corners into 2D screen space
for(sdword i=0;i<num;i++)
{
HPoint Projected;
vertexBox[Outline[i]].ProjectToScreen(width, height, mat, Projected);
dst[i] = Projected;
}
float Sum = (dst[num-1][0] - dst[0][0]) * (dst[num-1][1] + dst[0][1]);
for(int i=0; i<num-1; i++)
Sum += (dst[i][0] - dst[i+1][0]) * (dst[i][1] + dst[i+1][1]);
return Sum * 0.5f; //return computed value corrected by 0.5
}

505
ode/OPCODE/Ice/IceAABB.h Normal file
View File

@ -0,0 +1,505 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains AABB-related code. (axis-aligned bounding box)
* \file IceAABB.h
* \author Pierre Terdiman
* \date January, 13, 2000
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __ICEAABB_H__
#define __ICEAABB_H__
// Forward declarations
class Sphere;
//! Declarations of type-independent methods (most of them implemented in the .cpp)
#define AABB_COMMON_METHODS \
AABB& Add(const AABB& aabb); \
float MakeCube(AABB& cube) const; \
void MakeSphere(Sphere& sphere) const; \
const sbyte* ComputeOutline(const Point& local_eye, sdword& num) const; \
float ComputeBoxArea(const Point& eye, const Matrix4x4& mat, float width, float height, sdword& num) const; \
bool IsInside(const AABB& box) const; \
bool ComputePlanes(Plane* planes) const; \
bool ComputePoints(Point* pts) const; \
const Point* GetVertexNormals() const; \
const udword* GetEdges() const; \
const Point* GetEdgeNormals() const; \
inline_ BOOL ContainsPoint(const Point& p) const \
{ \
if(p.x > GetMax(0) || p.x < GetMin(0)) return FALSE; \
if(p.y > GetMax(1) || p.y < GetMin(1)) return FALSE; \
if(p.z > GetMax(2) || p.z < GetMin(2)) return FALSE; \
return TRUE; \
}
enum AABBType
{
AABB_RENDER = 0, //!< AABB used for rendering. Not visible == not rendered.
AABB_UPDATE = 1, //!< AABB used for dynamic updates. Not visible == not updated.
AABB_FORCE_DWORD = 0x7fffffff,
};
#ifdef USE_MINMAX
struct ICEMATHS_API ShadowAABB
{
Point mMin;
Point mMax;
};
class ICEMATHS_API AABB
{
public:
//! Constructor
inline_ AABB() {}
//! Destructor
inline_ ~AABB() {}
//! Type-independent methods
AABB_COMMON_METHODS;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Setups an AABB from min & max vectors.
* \param min [in] the min point
* \param max [in] the max point
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void SetMinMax(const Point& min, const Point& max) { mMin = min; mMax = max; }
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Setups an AABB from center & extents vectors.
* \param c [in] the center point
* \param e [in] the extents vector
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void SetCenterExtents(const Point& c, const Point& e) { mMin = c - e; mMax = c + e; }
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Setups an empty AABB.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void SetEmpty() { Point p(MIN_FLOAT, MIN_FLOAT, MIN_FLOAT); mMin = -p; mMax = p;}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Setups a point AABB.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void SetPoint(const Point& pt) { mMin = mMax = pt; }
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Gets the size of the AABB. The size is defined as the longest extent.
* \return the size of the AABB
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
float GetSize() const { Point e; GetExtents(e); return e.Max(); }
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Extends the AABB.
* \param p [in] the next point
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void Extend(const Point& p)
{
if(p.x > mMax.x) mMax.x = p.x;
if(p.x < mMin.x) mMin.x = p.x;
if(p.y > mMax.y) mMax.y = p.y;
if(p.y < mMin.y) mMin.y = p.y;
if(p.z > mMax.z) mMax.z = p.z;
if(p.z < mMin.z) mMin.z = p.z;
}
// Data access
//! Get min point of the box
inline_ void GetMin(Point& min) const { min = mMin; }
//! Get max point of the box
inline_ void GetMax(Point& max) const { max = mMax; }
//! Get component of the box's min point along a given axis
inline_ float GetMin(udword axis) const { return mMin[axis]; }
//! Get component of the box's max point along a given axis
inline_ float GetMax(udword axis) const { return mMax[axis]; }
//! Get box center
inline_ void GetCenter(Point& center) const { center = (mMax + mMin)*0.5f; }
//! Get box extents
inline_ void GetExtents(Point& extents) const { extents = (mMax - mMin)*0.5f; }
//! Get component of the box's center along a given axis
inline_ float GetCenter(udword axis) const { return (mMax[axis] + mMin[axis])*0.5f; }
//! Get component of the box's extents along a given axis
inline_ float GetExtents(udword axis) const { return (mMax[axis] - mMin[axis])*0.5f; }
//! Get box diagonal
inline_ void GetDiagonal(Point& diagonal) const { diagonal = mMax - mMin; }
inline_ float GetWidth() const { return mMax.x - mMin.x; }
inline_ float GetHeight() const { return mMax.y - mMin.y; }
inline_ float GetDepth() const { return mMax.z - mMin.z; }
//! Volume
inline_ float GetVolume() const { return GetWidth() * GetHeight() * GetDepth(); }
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the intersection between two AABBs.
* \param a [in] the other AABB
* \return true on intersection
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ BOOL Intersect(const AABB& a) const
{
if(mMax.x < a.mMin.x
|| a.mMax.x < mMin.x
|| mMax.y < a.mMin.y
|| a.mMax.y < mMin.y
|| mMax.z < a.mMin.z
|| a.mMax.z < mMin.z) return FALSE;
return TRUE;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the 1D-intersection between two AABBs, on a given axis.
* \param a [in] the other AABB
* \param axis [in] the axis (0, 1, 2)
* \return true on intersection
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ BOOL Intersect(const AABB& a, udword axis) const
{
if(mMax[axis] < a.mMin[axis] || a.mMax[axis] < mMin[axis]) return FALSE;
return TRUE;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recomputes the AABB after an arbitrary transform by a 4x4 matrix.
* Original code by Charles Bloom on the GD-Algorithm list. (I slightly modified it)
* \param mtx [in] the transform matrix
* \param aabb [out] the transformed AABB [can be *this]
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ void Rotate(const Matrix4x4& mtx, AABB& aabb) const
{
// The three edges transformed: you can efficiently transform an X-only vector
// by just getting the "X" column of the matrix
Point vx,vy,vz;
mtx.GetRow(0, vx); vx *= (mMax.x - mMin.x);
mtx.GetRow(1, vy); vy *= (mMax.y - mMin.y);
mtx.GetRow(2, vz); vz *= (mMax.z - mMin.z);
// Transform the min point
aabb.mMin = aabb.mMax = mMin * mtx;
// Take the transformed min & axes and find new extents
// Using CPU code in the right place is faster...
if(IS_NEGATIVE_FLOAT(vx.x)) aabb.mMin.x += vx.x; else aabb.mMax.x += vx.x;
if(IS_NEGATIVE_FLOAT(vx.y)) aabb.mMin.y += vx.y; else aabb.mMax.y += vx.y;
if(IS_NEGATIVE_FLOAT(vx.z)) aabb.mMin.z += vx.z; else aabb.mMax.z += vx.z;
if(IS_NEGATIVE_FLOAT(vy.x)) aabb.mMin.x += vy.x; else aabb.mMax.x += vy.x;
if(IS_NEGATIVE_FLOAT(vy.y)) aabb.mMin.y += vy.y; else aabb.mMax.y += vy.y;
if(IS_NEGATIVE_FLOAT(vy.z)) aabb.mMin.z += vy.z; else aabb.mMax.z += vy.z;
if(IS_NEGATIVE_FLOAT(vz.x)) aabb.mMin.x += vz.x; else aabb.mMax.x += vz.x;
if(IS_NEGATIVE_FLOAT(vz.y)) aabb.mMin.y += vz.y; else aabb.mMax.y += vz.y;
if(IS_NEGATIVE_FLOAT(vz.z)) aabb.mMin.z += vz.z; else aabb.mMax.z += vz.z;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Checks the AABB is valid.
* \return true if the box is valid
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ BOOL IsValid() const
{
// Consistency condition for (Min, Max) boxes: min < max
if(mMin.x > mMax.x) return FALSE;
if(mMin.y > mMax.y) return FALSE;
if(mMin.z > mMax.z) return FALSE;
return TRUE;
}
//! Operator for AABB *= float. Scales the extents, keeps same center.
inline_ AABB& operator*=(float s)
{
Point Center; GetCenter(Center);
Point Extents; GetExtents(Extents);
SetCenterExtents(Center, Extents * s);
return *this;
}
//! Operator for AABB /= float. Scales the extents, keeps same center.
inline_ AABB& operator/=(float s)
{
Point Center; GetCenter(Center);
Point Extents; GetExtents(Extents);
SetCenterExtents(Center, Extents / s);
return *this;
}
//! Operator for AABB += Point. Translates the box.
inline_ AABB& operator+=(const Point& trans)
{
mMin+=trans;
mMax+=trans;
return *this;
}
private:
Point mMin; //!< Min point
Point mMax; //!< Max point
};
#else
class ICEMATHS_API AABB
{
public:
//! Constructor
inline_ AABB() {}
//! Destructor
inline_ ~AABB() {}
//! Type-independent methods
AABB_COMMON_METHODS;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Setups an AABB from min & max vectors.
* \param min [in] the min point
* \param max [in] the max point
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void SetMinMax(const Point& min, const Point& max) { mCenter = (max + min)*0.5f; mExtents = (max - min)*0.5f; }
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Setups an AABB from center & extents vectors.
* \param c [in] the center point
* \param e [in] the extents vector
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void SetCenterExtents(const Point& c, const Point& e) { mCenter = c; mExtents = e; }
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Setups an empty AABB.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void SetEmpty() { mCenter.Zero(); mExtents.Set(MIN_FLOAT, MIN_FLOAT, MIN_FLOAT);}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Setups a point AABB.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void SetPoint(const Point& pt) { mCenter = pt; mExtents.Zero(); }
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Gets the size of the AABB. The size is defined as the longest extent.
* \return the size of the AABB
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
float GetSize() const { return mExtents.Max(); }
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Extends the AABB.
* \param p [in] the next point
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void Extend(const Point& p)
{
Point Max = mCenter + mExtents;
Point Min = mCenter - mExtents;
if(p.x > Max.x) Max.x = p.x;
if(p.x < Min.x) Min.x = p.x;
if(p.y > Max.y) Max.y = p.y;
if(p.y < Min.y) Min.y = p.y;
if(p.z > Max.z) Max.z = p.z;
if(p.z < Min.z) Min.z = p.z;
SetMinMax(Min, Max);
}
// Data access
//! Get min point of the box
inline_ void GetMin(Point& min) const { min = mCenter - mExtents; }
//! Get max point of the box
inline_ void GetMax(Point& max) const { max = mCenter + mExtents; }
//! Get component of the box's min point along a given axis
inline_ float GetMin(udword axis) const { return mCenter[axis] - mExtents[axis]; }
//! Get component of the box's max point along a given axis
inline_ float GetMax(udword axis) const { return mCenter[axis] + mExtents[axis]; }
//! Get box center
inline_ void GetCenter(Point& center) const { center = mCenter; }
//! Get box extents
inline_ void GetExtents(Point& extents) const { extents = mExtents; }
//! Get component of the box's center along a given axis
inline_ float GetCenter(udword axis) const { return mCenter[axis]; }
//! Get component of the box's extents along a given axis
inline_ float GetExtents(udword axis) const { return mExtents[axis]; }
//! Get box diagonal
inline_ void GetDiagonal(Point& diagonal) const { diagonal = mExtents * 2.0f; }
inline_ float GetWidth() const { return mExtents.x * 2.0f; }
inline_ float GetHeight() const { return mExtents.y * 2.0f; }
inline_ float GetDepth() const { return mExtents.z * 2.0f; }
//! Volume
inline_ float GetVolume() const { return mExtents.x * mExtents.y * mExtents.z * 8.0f; }
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the intersection between two AABBs.
* \param a [in] the other AABB
* \return true on intersection
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ BOOL Intersect(const AABB& a) const
{
float tx = mCenter.x - a.mCenter.x; float ex = a.mExtents.x + mExtents.x; if(AIR(tx) > IR(ex)) return FALSE;
float ty = mCenter.y - a.mCenter.y; float ey = a.mExtents.y + mExtents.y; if(AIR(ty) > IR(ey)) return FALSE;
float tz = mCenter.z - a.mCenter.z; float ez = a.mExtents.z + mExtents.z; if(AIR(tz) > IR(ez)) return FALSE;
return TRUE;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* The standard intersection method from Gamasutra. Just here to check its speed against the one above.
* \param a [in] the other AABB
* \return true on intersection
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ bool GomezIntersect(const AABB& a)
{
Point T = mCenter - a.mCenter; // Vector from A to B
return ((fabsf(T.x) <= (a.mExtents.x + mExtents.x))
&& (fabsf(T.y) <= (a.mExtents.y + mExtents.y))
&& (fabsf(T.z) <= (a.mExtents.z + mExtents.z)));
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the 1D-intersection between two AABBs, on a given axis.
* \param a [in] the other AABB
* \param axis [in] the axis (0, 1, 2)
* \return true on intersection
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ BOOL Intersect(const AABB& a, udword axis) const
{
float t = mCenter[axis] - a.mCenter[axis];
float e = a.mExtents[axis] + mExtents[axis];
if(AIR(t) > IR(e)) return FALSE;
return TRUE;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recomputes the AABB after an arbitrary transform by a 4x4 matrix.
* \param mtx [in] the transform matrix
* \param aabb [out] the transformed AABB [can be *this]
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ void Rotate(const Matrix4x4& mtx, AABB& aabb) const
{
// Compute new center
aabb.mCenter = mCenter * mtx;
// Compute new extents. FPU code & CPU code have been interleaved for improved performance.
Point Ex(mtx.m[0][0] * mExtents.x, mtx.m[0][1] * mExtents.x, mtx.m[0][2] * mExtents.x);
IR(Ex.x)&=0x7fffffff; IR(Ex.y)&=0x7fffffff; IR(Ex.z)&=0x7fffffff;
Point Ey(mtx.m[1][0] * mExtents.y, mtx.m[1][1] * mExtents.y, mtx.m[1][2] * mExtents.y);
IR(Ey.x)&=0x7fffffff; IR(Ey.y)&=0x7fffffff; IR(Ey.z)&=0x7fffffff;
Point Ez(mtx.m[2][0] * mExtents.z, mtx.m[2][1] * mExtents.z, mtx.m[2][2] * mExtents.z);
IR(Ez.x)&=0x7fffffff; IR(Ez.y)&=0x7fffffff; IR(Ez.z)&=0x7fffffff;
aabb.mExtents.x = Ex.x + Ey.x + Ez.x;
aabb.mExtents.y = Ex.y + Ey.y + Ez.y;
aabb.mExtents.z = Ex.z + Ey.z + Ez.z;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Checks the AABB is valid.
* \return true if the box is valid
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ BOOL IsValid() const
{
// Consistency condition for (Center, Extents) boxes: Extents >= 0
if(IS_NEGATIVE_FLOAT(mExtents.x)) return FALSE;
if(IS_NEGATIVE_FLOAT(mExtents.y)) return FALSE;
if(IS_NEGATIVE_FLOAT(mExtents.z)) return FALSE;
return TRUE;
}
//! Operator for AABB *= float. Scales the extents, keeps same center.
inline_ AABB& operator*=(float s) { mExtents*=s; return *this; }
//! Operator for AABB /= float. Scales the extents, keeps same center.
inline_ AABB& operator/=(float s) { mExtents/=s; return *this; }
//! Operator for AABB += Point. Translates the box.
inline_ AABB& operator+=(const Point& trans)
{
mCenter+=trans;
return *this;
}
private:
Point mCenter; //!< AABB Center
Point mExtents; //!< x, y and z extents
};
#endif
inline_ void ComputeMinMax(const Point& p, Point& min, Point& max)
{
if(p.x > max.x) max.x = p.x;
if(p.x < min.x) min.x = p.x;
if(p.y > max.y) max.y = p.y;
if(p.y < min.y) min.y = p.y;
if(p.z > max.z) max.z = p.z;
if(p.z < min.z) min.z = p.z;
}
inline_ void ComputeAABB(AABB& aabb, const Point* list, udword nb_pts)
{
if(list)
{
Point Maxi(MIN_FLOAT, MIN_FLOAT, MIN_FLOAT);
Point Mini(MAX_FLOAT, MAX_FLOAT, MAX_FLOAT);
while(nb_pts--)
{
// _prefetch(list+1); // off by one ?
ComputeMinMax(*list++, Mini, Maxi);
}
aabb.SetMinMax(Mini, Maxi);
}
}
#endif // __ICEAABB_H__

54
ode/OPCODE/Ice/IceAxes.h Normal file
View File

@ -0,0 +1,54 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains axes definition.
* \file IceAxes.h
* \author Pierre Terdiman
* \date January, 29, 2000
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __ICEAXES_H__
#define __ICEAXES_H__
enum PointComponent
{
X = 0,
Y = 1,
Z = 2,
W = 3,
FORCE_DWORD = 0x7fffffff
};
enum AxisOrder
{
AXES_XYZ = (X)|(Y<<2)|(Z<<4),
AXES_XZY = (X)|(Z<<2)|(Y<<4),
AXES_YXZ = (Y)|(X<<2)|(Z<<4),
AXES_YZX = (Y)|(Z<<2)|(X<<4),
AXES_ZXY = (Z)|(X<<2)|(Y<<4),
AXES_ZYX = (Z)|(Y<<2)|(X<<4),
AXES_FORCE_DWORD = 0x7fffffff
};
class ICEMATHS_API Axes
{
public:
inline_ Axes(AxisOrder order)
{
mAxis0 = (order ) & 3;
mAxis1 = (order>>2) & 3;
mAxis2 = (order>>4) & 3;
}
inline_ ~Axes() {}
udword mAxis0;
udword mAxis1;
udword mAxis2;
};
#endif // __ICEAXES_H__

142
ode/OPCODE/Ice/IceBoundingSphere.h Normal file
View File

@ -0,0 +1,142 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code to compute the minimal bounding sphere.
* \file IceBoundingSphere.h
* \author Pierre Terdiman
* \date January, 29, 2000
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __ICEBOUNDINGSPHERE_H__
#define __ICEBOUNDINGSPHERE_H__
enum BSphereMethod
{
BS_NONE,
BS_GEMS,
BS_MINIBALL,
BS_FORCE_DWORD = 0x7fffffff
};
class ICEMATHS_API Sphere
{
public:
//! Constructor
inline_ Sphere() {}
//! Constructor
inline_ Sphere(const Point& center, float radius) : mCenter(center), mRadius(radius) {}
//! Constructor
Sphere(udword nb_verts, const Point* verts);
//! Copy constructor
inline_ Sphere(const Sphere& sphere) : mCenter(sphere.mCenter), mRadius(sphere.mRadius) {}
//! Destructor
inline_ ~Sphere() {}
BSphereMethod Compute(udword nb_verts, const Point* verts);
bool FastCompute(udword nb_verts, const Point* verts);
// Access methods
inline_ const Point& GetCenter() const { return mCenter; }
inline_ float GetRadius() const { return mRadius; }
inline_ const Point& Center() const { return mCenter; }
inline_ float Radius() const { return mRadius; }
inline_ Sphere& Set(const Point& center, float radius) { mCenter = center; mRadius = radius; return *this; }
inline_ Sphere& SetCenter(const Point& center) { mCenter = center; return *this; }
inline_ Sphere& SetRadius(float radius) { mRadius = radius; return *this; }
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Tests if a point is contained within the sphere.
* \param p [in] the point to test
* \return true if inside the sphere
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ bool Contains(const Point& p) const
{
return mCenter.SquareDistance(p) <= mRadius*mRadius;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Tests if a sphere is contained within the sphere.
* \param sphere [in] the sphere to test
* \return true if inside the sphere
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ bool Contains(const Sphere& sphere) const
{
// If our radius is the smallest, we can't possibly contain the other sphere
if(mRadius < sphere.mRadius) return false;
// So r is always positive or null now
float r = mRadius - sphere.mRadius;
return mCenter.SquareDistance(sphere.mCenter) <= r*r;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Tests if a box is contained within the sphere.
* \param aabb [in] the box to test
* \return true if inside the sphere
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ BOOL Contains(const AABB& aabb) const
{
// I assume if all 8 box vertices are inside the sphere, so does the whole box.
// Sounds ok but maybe there's a better way?
float R2 = mRadius * mRadius;
#ifdef USE_MIN_MAX
const Point& Max = ((ShadowAABB&)&aabb).mMax;
const Point& Min = ((ShadowAABB&)&aabb).mMin;
#else
Point Max; aabb.GetMax(Max);
Point Min; aabb.GetMin(Min);
#endif
Point p;
p.x=Max.x; p.y=Max.y; p.z=Max.z; if(mCenter.SquareDistance(p)>=R2) return FALSE;
p.x=Min.x; if(mCenter.SquareDistance(p)>=R2) return FALSE;
p.x=Max.x; p.y=Min.y; if(mCenter.SquareDistance(p)>=R2) return FALSE;
p.x=Min.x; if(mCenter.SquareDistance(p)>=R2) return FALSE;
p.x=Max.x; p.y=Max.y; p.z=Min.z; if(mCenter.SquareDistance(p)>=R2) return FALSE;
p.x=Min.x; if(mCenter.SquareDistance(p)>=R2) return FALSE;
p.x=Max.x; p.y=Min.y; if(mCenter.SquareDistance(p)>=R2) return FALSE;
p.x=Min.x; if(mCenter.SquareDistance(p)>=R2) return FALSE;
return TRUE;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Tests if the sphere intersects another sphere
* \param sphere [in] the other sphere
* \return true if spheres overlap
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ bool Intersect(const Sphere& sphere) const
{
float r = mRadius + sphere.mRadius;
return mCenter.SquareDistance(sphere.mCenter) <= r*r;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Checks the sphere is valid.
* \return true if the box is valid
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ BOOL IsValid() const
{
// Consistency condition for spheres: Radius >= 0.0f
if(mRadius < 0.0f) return FALSE;
return TRUE;
}
public:
Point mCenter; //!< Sphere center
float mRadius; //!< Sphere radius
};
#endif // __ICEBOUNDINGSPHERE_H__

345
ode/OPCODE/Ice/IceContainer.cpp Normal file
View File

@ -0,0 +1,345 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains a simple container class.
* \file IceContainer.cpp
* \author Pierre Terdiman
* \date February, 5, 2000
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains a list of 32-bits values.
* Use this class when you need to store an unknown number of values. The list is automatically
* resized and can contains 32-bits entities (dwords or floats)
*
* \class Container
* \author Pierre Terdiman
* \version 1.0
* \date 08.15.98
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Precompiled Header
#include "Stdafx.h"
using namespace IceCore;
// Static members
#ifdef CONTAINER_STATS
udword Container::mNbContainers = 0;
udword Container::mUsedRam = 0;
#endif
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Constructor. No entries allocated there.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Container::Container() : mMaxNbEntries(0), mCurNbEntries(0), mEntries(null), mGrowthFactor(2.0f)
{
#ifdef CONTAINER_STATS
mNbContainers++;
mUsedRam+=sizeof(Container);
#endif
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Constructor. Also allocates a given number of entries.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Container::Container(udword size, float growth_factor) : mMaxNbEntries(0), mCurNbEntries(0), mEntries(null), mGrowthFactor(growth_factor)
{
#ifdef CONTAINER_STATS
mNbContainers++;
mUsedRam+=sizeof(Container);
#endif
SetSize(size);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Copy constructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Container::Container(const Container& object) : mMaxNbEntries(0), mCurNbEntries(0), mEntries(null), mGrowthFactor(2.0f)
{
#ifdef CONTAINER_STATS
mNbContainers++;
mUsedRam+=sizeof(Container);
#endif
*this = object;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Destructor. Frees everything and leaves.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Container::~Container()
{
Empty();
#ifdef CONTAINER_STATS
mNbContainers--;
mUsedRam-=GetUsedRam();
#endif
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Clears the container. All stored values are deleted, and it frees used ram.
* \see Reset()
* \return Self-Reference
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Container& Container::Empty()
{
#ifdef CONTAINER_STATS
mUsedRam-=mMaxNbEntries*sizeof(udword);
#endif
DELETEARRAY(mEntries);
mCurNbEntries = mMaxNbEntries = 0;
return *this;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Resizes the container.
* \param needed [in] assume the container can be added at least "needed" values
* \return true if success.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool Container::Resize(udword needed)
{
#ifdef CONTAINER_STATS
// Subtract previous amount of bytes
mUsedRam-=mMaxNbEntries*sizeof(udword);
#endif
// Get more entries
mMaxNbEntries = mMaxNbEntries ? udword(float(mMaxNbEntries)*mGrowthFactor) : 2; // Default nb Entries = 2
if(mMaxNbEntries<mCurNbEntries + needed) mMaxNbEntries = mCurNbEntries + needed;
// Get some bytes for new entries
udword* NewEntries = new udword[mMaxNbEntries];
CHECKALLOC(NewEntries);
#ifdef CONTAINER_STATS
// Add current amount of bytes
mUsedRam+=mMaxNbEntries*sizeof(udword);
#endif
// Copy old data if needed
if(mCurNbEntries) CopyMemory(NewEntries, mEntries, mCurNbEntries*sizeof(udword));
// Delete old data
DELETEARRAY(mEntries);
// Assign new pointer
mEntries = NewEntries;
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Sets the initial size of the container. If it already contains something, it's discarded.
* \param nb [in] Number of entries
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool Container::SetSize(udword nb)
{
// Make sure it's empty
Empty();
// Checkings
if(!nb) return false;
// Initialize for nb entries
mMaxNbEntries = nb;
// Get some bytes for new entries
mEntries = new udword[mMaxNbEntries];
CHECKALLOC(mEntries);
#ifdef CONTAINER_STATS
// Add current amount of bytes
mUsedRam+=mMaxNbEntries*sizeof(udword);
#endif
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Refits the container and get rid of unused bytes.
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool Container::Refit()
{
#ifdef CONTAINER_STATS
// Subtract previous amount of bytes
mUsedRam-=mMaxNbEntries*sizeof(udword);
#endif
// Get just enough entries
mMaxNbEntries = mCurNbEntries;
if(!mMaxNbEntries) return false;
// Get just enough bytes
udword* NewEntries = new udword[mMaxNbEntries];
CHECKALLOC(NewEntries);
#ifdef CONTAINER_STATS
// Add current amount of bytes
mUsedRam+=mMaxNbEntries*sizeof(udword);
#endif
// Copy old data
CopyMemory(NewEntries, mEntries, mCurNbEntries*sizeof(udword));
// Delete old data
DELETEARRAY(mEntries);
// Assign new pointer
mEntries = NewEntries;
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Checks whether the container already contains a given value.
* \param entry [in] the value to look for in the container
* \param location [out] a possible pointer to store the entry location
* \see Add(udword entry)
* \see Add(float entry)
* \see Empty()
* \return true if the value has been found in the container, else false.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool Container::Contains(udword entry, udword* location) const
{
// Look for the entry
for(udword i=0;i<mCurNbEntries;i++)
{
if(mEntries[i]==entry)
{
if(location) *location = i;
return true;
}
}
return false;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Deletes an entry. If the container contains such an entry, it's removed.
* \param entry [in] the value to delete.
* \return true if the value has been found in the container, else false.
* \warning This method is arbitrary slow (O(n)) and should be used carefully. Insertion order is not preserved.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool Container::Delete(udword entry)
{
// Look for the entry
for(udword i=0;i<mCurNbEntries;i++)
{
if(mEntries[i]==entry)
{
// Entry has been found at index i. The strategy is to copy the last current entry at index i, and decrement the current number of entries.
DeleteIndex(i);
return true;
}
}
return false;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Deletes an entry, preserving the insertion order. If the container contains such an entry, it's removed.
* \param entry [in] the value to delete.
* \return true if the value has been found in the container, else false.
* \warning This method is arbitrary slow (O(n)) and should be used carefully.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool Container::DeleteKeepingOrder(udword entry)
{
// Look for the entry
for(udword i=0;i<mCurNbEntries;i++)
{
if(mEntries[i]==entry)
{
// Entry has been found at index i.
// Shift entries to preserve order. You really should use a linked list instead.
mCurNbEntries--;
for(udword j=i;j<mCurNbEntries;j++)
{
mEntries[j] = mEntries[j+1];
}
return true;
}
}
return false;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Gets the next entry, starting from input one.
* \param entry [in/out] On input, the entry to look for. On output, the next entry
* \param find_mode [in] wrap/clamp
* \return Self-Reference
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Container& Container::FindNext(udword& entry, FindMode find_mode)
{
udword Location;
if(Contains(entry, &Location))
{
Location++;
if(Location==mCurNbEntries) Location = find_mode==FIND_WRAP ? 0 : mCurNbEntries-1;
entry = mEntries[Location];
}
return *this;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Gets the previous entry, starting from input one.
* \param entry [in/out] On input, the entry to look for. On output, the previous entry
* \param find_mode [in] wrap/clamp
* \return Self-Reference
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Container& Container::FindPrev(udword& entry, FindMode find_mode)
{
udword Location;
if(Contains(entry, &Location))
{
Location--;
if(Location==0xffffffff) Location = find_mode==FIND_WRAP ? mCurNbEntries-1 : 0;
entry = mEntries[Location];
}
return *this;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Gets the ram used by the container.
* \return the ram used in bytes.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
udword Container::GetUsedRam() const
{
return sizeof(Container) + mMaxNbEntries * sizeof(udword);
}
/*void Container::operator=(const Container& object)
{
SetSize(object.GetNbEntries());
CopyMemory(mEntries, object.GetEntries(), mMaxNbEntries*sizeof(udword));
mCurNbEntries = mMaxNbEntries;
}*/

212
ode/OPCODE/Ice/IceContainer.h Normal file
View File

@ -0,0 +1,212 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains a simple container class.
* \file IceContainer.h
* \author Pierre Terdiman
* \date February, 5, 2000
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __ICECONTAINER_H__
#define __ICECONTAINER_H__
#define CONTAINER_STATS
enum FindMode
{
FIND_CLAMP,
FIND_WRAP,
FIND_FORCE_DWORD = 0x7fffffff
};
class ICECORE_API Container
{
public:
// Constructor / Destructor
Container();
Container(const Container& object);
Container(udword size, float growth_factor);
~Container();
// Management
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* A O(1) method to add a value in the container. The container is automatically resized if needed.
* The method is inline, not the resize. The call overhead happens on resizes only, which is not a problem since the resizing operation
* costs a lot more than the call overhead...
*
* \param entry [in] a udword to store in the container
* \see Add(float entry)
* \see Empty()
* \see Contains(udword entry)
* \return Self-Reference
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ Container& Add(udword entry)
{
// Resize if needed
if(mCurNbEntries==mMaxNbEntries) Resize();
// Add new entry
mEntries[mCurNbEntries++] = entry;
return *this;
}
inline_ Container& Add(const udword* entries, udword nb)
{
// Resize if needed
if(mCurNbEntries+nb>mMaxNbEntries) Resize(nb);
// Add new entry
CopyMemory(&mEntries[mCurNbEntries], entries, nb*sizeof(udword));
mCurNbEntries+=nb;
return *this;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* A O(1) method to add a value in the container. The container is automatically resized if needed.
* The method is inline, not the resize. The call overhead happens on resizes only, which is not a problem since the resizing operation
* costs a lot more than the call overhead...
*
* \param entry [in] a float to store in the container
* \see Add(udword entry)
* \see Empty()
* \see Contains(udword entry)
* \return Self-Reference
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ Container& Add(float entry)
{
// Resize if needed
if(mCurNbEntries==mMaxNbEntries) Resize();
// Add new entry
mEntries[mCurNbEntries++] = IR(entry);
return *this;
}
inline_ Container& Add(const float* entries, udword nb)
{
// Resize if needed
if(mCurNbEntries+nb>mMaxNbEntries) Resize(nb);
// Add new entry
CopyMemory(&mEntries[mCurNbEntries], entries, nb*sizeof(float));
mCurNbEntries+=nb;
return *this;
}
//! Add unique [slow]
inline_ Container& AddUnique(udword entry)
{
if(!Contains(entry)) Add(entry);
return *this;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Clears the container. All stored values are deleted, and it frees used ram.
* \see Reset()
* \return Self-Reference
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Container& Empty();
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Resets the container. Stored values are discarded but the buffer is kept so that further calls don't need resizing again.
* That's a kind of temporal coherence.
* \see Empty()
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ void Reset()
{
// Avoid the write if possible
// ### CMOV
if(mCurNbEntries) mCurNbEntries = 0;
}
// HANDLE WITH CARE
inline_ void ForceSize(udword size)
{
mCurNbEntries = size;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Sets the initial size of the container. If it already contains something, it's discarded.
* \param nb [in] Number of entries
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool SetSize(udword nb);
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Refits the container and get rid of unused bytes.
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool Refit();
// Checks whether the container already contains a given value.
bool Contains(udword entry, udword* location=null) const;
// Deletes an entry - doesn't preserve insertion order.
bool Delete(udword entry);
// Deletes an entry - does preserve insertion order.
bool DeleteKeepingOrder(udword entry);
//! Deletes the very last entry.
inline_ void DeleteLastEntry() { if(mCurNbEntries) mCurNbEntries--; }
//! Deletes the entry whose index is given
inline_ void DeleteIndex(udword index) { mEntries[index] = mEntries[--mCurNbEntries]; }
// Helpers
Container& FindNext(udword& entry, FindMode find_mode=FIND_CLAMP);
Container& FindPrev(udword& entry, FindMode find_mode=FIND_CLAMP);
// Data access.
inline_ udword GetNbEntries() const { return mCurNbEntries; } //!< Returns the current number of entries.
inline_ udword GetEntry(udword i) const { return mEntries[i]; } //!< Returns ith entry
inline_ udword* GetEntries() const { return mEntries; } //!< Returns the list of entries.
inline_ udword GetFirst() const { return mEntries[0]; }
inline_ udword GetLast() const { return mEntries[mCurNbEntries-1]; }
// Growth control
inline_ float GetGrowthFactor() const { return mGrowthFactor; } //!< Returns the growth factor
inline_ void SetGrowthFactor(float growth) { mGrowthFactor = growth; } //!< Sets the growth factor
inline_ bool IsFull() const { return mCurNbEntries==mMaxNbEntries; } //!< Checks the container is full
inline_ BOOL IsNotEmpty() const { return mCurNbEntries; } //!< Checks the container is empty
//! Read-access as an array
inline_ udword operator[](udword i) const { ASSERT(i>=0 && i<mCurNbEntries); return mEntries[i]; }
//! Write-access as an array
inline_ udword& operator[](udword i) { ASSERT(i>=0 && i<mCurNbEntries); return mEntries[i]; }
// Stats
udword GetUsedRam() const;
//! Operator for "Container A = Container B"
//void operator = (const Container& object);
#ifdef CONTAINER_STATS
inline_ udword GetNbContainers() const { return mNbContainers; }
inline_ udword GetTotalBytes() const { return mUsedRam; }
private:
static udword mNbContainers; //!< Number of containers around
static udword mUsedRam; //!< Amount of bytes used by containers in the system
#endif
private:
// Resizing
bool Resize(udword needed=1);
// Data
udword mMaxNbEntries; //!< Maximum possible number of entries
udword mCurNbEntries; //!< Current number of entries
udword* mEntries; //!< List of entries
float mGrowthFactor; //!< Resize: new number of entries = old number * mGrowthFactor
};
#endif // __ICECONTAINER_H__

337
ode/OPCODE/Ice/IceFPU.h Normal file
View File

@ -0,0 +1,337 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains FPU related code.
* \file IceFPU.h
* \author Pierre Terdiman
* \date April, 4, 2000
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __ICEFPU_H__
#define __ICEFPU_H__
#define SIGN_BITMASK 0x80000000
//! Integer representation of a floating-point value.
#define IR(x) ((udword&)(x))
//! Signed integer representation of a floating-point value.
#define SIR(x) ((sdword&)(x))
//! Absolute integer representation of a floating-point value
#define AIR(x) (IR(x)&0x7fffffff)
//! Floating-point representation of an integer value.
#define FR(x) ((float&)(x))
//! Integer-based comparison of a floating point value.
//! Don't use it blindly, it can be faster or slower than the FPU comparison, depends on the context.
#define IS_NEGATIVE_FLOAT(x) (IR(x)&0x80000000)
//! Fast fabs for floating-point values. It just clears the sign bit.
//! Don't use it blindy, it can be faster or slower than the FPU comparison, depends on the context.
inline_ float FastFabs(float x)
{
udword FloatBits = IR(x)&0x7fffffff;
return FR(FloatBits);
}
//! Fast square root for floating-point values.
inline_ float FastSqrt(float square)
{
#ifdef _MSC_VER
float retval;
__asm {
mov eax, square
sub eax, 0x3F800000
sar eax, 1
add eax, 0x3F800000
mov [retval], eax
}
return retval;
#else
return sqrt(square);
#endif
}
//! Saturates positive to zero.
inline_ float fsat(float f)
{
udword y = (udword&)f & ~((sdword&)f >>31);
return (float&)y;
}
//! Computes 1.0f / sqrtf(x).
inline_ float frsqrt(float f)
{
float x = f * 0.5f;
udword y = 0x5f3759df - ((udword&)f >> 1);
// Iteration...
(float&)y = (float&)y * ( 1.5f - ( x * (float&)y * (float&)y ) );
// Result
return (float&)y;
}
//! Computes 1.0f / sqrtf(x). Comes from NVIDIA.
inline_ float InvSqrt(const float& x)
{
udword tmp = (udword(IEEE_1_0 << 1) + IEEE_1_0 - *(udword*)&x) >> 1;
float y = *(float*)&tmp;
return y * (1.47f - 0.47f * x * y * y);
}
//! Computes 1.0f / sqrtf(x). Comes from Quake3. Looks like the first one I had above.
//! See http://www.magic-software.com/3DGEDInvSqrt.html
inline_ float RSqrt(float number)
{
long i;
float x2, y;
const float threehalfs = 1.5f;
x2 = number * 0.5f;
y = number;
i = * (long *) &y;
i = 0x5f3759df - (i >> 1);
y = * (float *) &i;
y = y * (threehalfs - (x2 * y * y));
return y;
}
//! TO BE DOCUMENTED
inline_ float fsqrt(float f)
{
udword y = ( ( (sdword&)f - 0x3f800000 ) >> 1 ) + 0x3f800000;
// Iteration...?
// (float&)y = (3.0f - ((float&)y * (float&)y) / f) * (float&)y * 0.5f;
// Result
return (float&)y;
}
//! Returns the float ranged espilon value.
inline_ float fepsilon(float f)
{
udword b = (udword&)f & 0xff800000;
udword a = b | 0x00000001;
(float&)a -= (float&)b;
// Result
return (float&)a;
}
//! Is the float valid ?
inline_ bool IsNAN(float value) { return (IR(value)&0x7f800000) == 0x7f800000; }
inline_ bool IsIndeterminate(float value) { return IR(value) == 0xffc00000; }
inline_ bool IsPlusInf(float value) { return IR(value) == 0x7f800000; }
inline_ bool IsMinusInf(float value) { return IR(value) == 0xff800000; }
inline_ bool IsValidFloat(float value)
{
if(IsNAN(value)) return false;
if(IsIndeterminate(value)) return false;
if(IsPlusInf(value)) return false;
if(IsMinusInf(value)) return false;
return true;
}
#define CHECK_VALID_FLOAT(x) ASSERT(IsValidFloat(x));
/*
//! FPU precision setting function.
inline_ void SetFPU()
{
// This function evaluates whether the floating-point
// control word is set to single precision/round to nearest/
// exceptions disabled. If these conditions don't hold, the
// function changes the control word to set them and returns
// TRUE, putting the old control word value in the passback
// location pointed to by pwOldCW.
{
uword wTemp, wSave;
__asm fstcw wSave
if (wSave & 0x300 || // Not single mode
0x3f != (wSave & 0x3f) || // Exceptions enabled
wSave & 0xC00) // Not round to nearest mode
{
__asm
{
mov ax, wSave
and ax, not 300h ;; single mode
or ax, 3fh ;; disable all exceptions
and ax, not 0xC00 ;; round to nearest mode
mov wTemp, ax
fldcw wTemp
}
}
}
}
*/
//! This function computes the slowest possible floating-point value (you can also directly use FLT_EPSILON)
inline_ float ComputeFloatEpsilon()
{
float f = 1.0f;
((udword&)f)^=1;
return f - 1.0f; // You can check it's the same as FLT_EPSILON
}
inline_ bool IsFloatZero(float x, float epsilon=1e-6f)
{
return x*x < epsilon;
}
#define FCOMI_ST0 _asm _emit 0xdb _asm _emit 0xf0
#define FCOMIP_ST0 _asm _emit 0xdf _asm _emit 0xf0
#define FCMOVB_ST0 _asm _emit 0xda _asm _emit 0xc0
#define FCMOVNB_ST0 _asm _emit 0xdb _asm _emit 0xc0
#define FCOMI_ST1 _asm _emit 0xdb _asm _emit 0xf1
#define FCOMIP_ST1 _asm _emit 0xdf _asm _emit 0xf1
#define FCMOVB_ST1 _asm _emit 0xda _asm _emit 0xc1
#define FCMOVNB_ST1 _asm _emit 0xdb _asm _emit 0xc1
#define FCOMI_ST2 _asm _emit 0xdb _asm _emit 0xf2
#define FCOMIP_ST2 _asm _emit 0xdf _asm _emit 0xf2
#define FCMOVB_ST2 _asm _emit 0xda _asm _emit 0xc2
#define FCMOVNB_ST2 _asm _emit 0xdb _asm _emit 0xc2
#define FCOMI_ST3 _asm _emit 0xdb _asm _emit 0xf3
#define FCOMIP_ST3 _asm _emit 0xdf _asm _emit 0xf3
#define FCMOVB_ST3 _asm _emit 0xda _asm _emit 0xc3
#define FCMOVNB_ST3 _asm _emit 0xdb _asm _emit 0xc3
#define FCOMI_ST4 _asm _emit 0xdb _asm _emit 0xf4
#define FCOMIP_ST4 _asm _emit 0xdf _asm _emit 0xf4
#define FCMOVB_ST4 _asm _emit 0xda _asm _emit 0xc4
#define FCMOVNB_ST4 _asm _emit 0xdb _asm _emit 0xc4
#define FCOMI_ST5 _asm _emit 0xdb _asm _emit 0xf5
#define FCOMIP_ST5 _asm _emit 0xdf _asm _emit 0xf5
#define FCMOVB_ST5 _asm _emit 0xda _asm _emit 0xc5
#define FCMOVNB_ST5 _asm _emit 0xdb _asm _emit 0xc5
#define FCOMI_ST6 _asm _emit 0xdb _asm _emit 0xf6
#define FCOMIP_ST6 _asm _emit 0xdf _asm _emit 0xf6
#define FCMOVB_ST6 _asm _emit 0xda _asm _emit 0xc6
#define FCMOVNB_ST6 _asm _emit 0xdb _asm _emit 0xc6
#define FCOMI_ST7 _asm _emit 0xdb _asm _emit 0xf7
#define FCOMIP_ST7 _asm _emit 0xdf _asm _emit 0xf7
#define FCMOVB_ST7 _asm _emit 0xda _asm _emit 0xc7
#define FCMOVNB_ST7 _asm _emit 0xdb _asm _emit 0xc7
//! A global function to find MAX(a,b) using FCOMI/FCMOV
inline_ float FCMax2(float a, float b)
{
#ifdef _MSC_VER
float Res;
_asm fld [a]
_asm fld [b]
FCOMI_ST1
FCMOVB_ST1
_asm fstp [Res]
_asm fcomp
return Res;
#else
return (a > b) ? a : b;
#endif
}
//! A global function to find MIN(a,b) using FCOMI/FCMOV
inline_ float FCMin2(float a, float b)
{
#ifdef _MSC_VER
float Res;
_asm fld [a]
_asm fld [b]
FCOMI_ST1
FCMOVNB_ST1
_asm fstp [Res]
_asm fcomp
return Res;
#else
return (a < b) ? a : b;
#endif
}
//! A global function to find MAX(a,b,c) using FCOMI/FCMOV
inline_ float FCMax3(float a, float b, float c)
{
#ifdef _MSC_VER
float Res;
_asm fld [a]
_asm fld [b]
_asm fld [c]
FCOMI_ST1
FCMOVB_ST1
FCOMI_ST2
FCMOVB_ST2
_asm fstp [Res]
_asm fcompp
return Res;
#else
return (a > b) ? ((a > c) ? a : c) : ((b > c) ? b : c);
#endif
}
//! A global function to find MIN(a,b,c) using FCOMI/FCMOV
inline_ float FCMin3(float a, float b, float c)
{
#ifdef _MSC_VER
float Res;
_asm fld [a]
_asm fld [b]
_asm fld [c]
FCOMI_ST1
FCMOVNB_ST1
FCOMI_ST2
FCMOVNB_ST2
_asm fstp [Res]
_asm fcompp
return Res;
#else
return (a < b) ? ((a < c) ? a : c) : ((b < c) ? b : c);
#endif
}
inline_ int ConvertToSortable(float f)
{
int& Fi = (int&)f;
int Fmask = (Fi>>31);
Fi ^= Fmask;
Fmask &= ~(1<<31);
Fi -= Fmask;
return Fi;
}
enum FPUMode
{
FPU_FLOOR = 0,
FPU_CEIL = 1,
FPU_BEST = 2,
FPU_FORCE_DWORD = 0x7fffffff
};
FUNCTION ICECORE_API FPUMode GetFPUMode();
FUNCTION ICECORE_API void SaveFPU();
FUNCTION ICECORE_API void RestoreFPU();
FUNCTION ICECORE_API void SetFPUFloorMode();
FUNCTION ICECORE_API void SetFPUCeilMode();
FUNCTION ICECORE_API void SetFPUBestMode();
FUNCTION ICECORE_API void SetFPUPrecision24();
FUNCTION ICECORE_API void SetFPUPrecision53();
FUNCTION ICECORE_API void SetFPUPrecision64();
FUNCTION ICECORE_API void SetFPURoundingChop();
FUNCTION ICECORE_API void SetFPURoundingUp();
FUNCTION ICECORE_API void SetFPURoundingDown();
FUNCTION ICECORE_API void SetFPURoundingNear();
FUNCTION ICECORE_API int intChop(const float& f);
FUNCTION ICECORE_API int intFloor(const float& f);
FUNCTION ICECORE_API int intCeil(const float& f);
#endif // __ICEFPU_H__

70
ode/OPCODE/Ice/IceHPoint.cpp Normal file
View File

@ -0,0 +1,70 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for homogeneous points.
* \file IceHPoint.cpp
* \author Pierre Terdiman
* \date April, 4, 2000
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Homogeneous point.
*
* Use it:
* - for clipping in homogeneous space (standard way)
* - to differentiate between points (w=1) and vectors (w=0).
* - in some cases you can also use it instead of Point for padding reasons.
*
* \class HPoint
* \author Pierre Terdiman
* \version 1.0
* \warning No cross-product in 4D.
* \warning HPoint *= Matrix3x3 doesn't exist, the matrix is first casted to a 4x4
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Precompiled Header
#include "Stdafx.h"
using namespace IceMaths;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Point Mul = HPoint * Matrix3x3;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Point HPoint::operator*(const Matrix3x3& mat) const
{
return Point(
x * mat.m[0][0] + y * mat.m[1][0] + z * mat.m[2][0],
x * mat.m[0][1] + y * mat.m[1][1] + z * mat.m[2][1],
x * mat.m[0][2] + y * mat.m[1][2] + z * mat.m[2][2] );
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// HPoint Mul = HPoint * Matrix4x4;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
HPoint HPoint::operator*(const Matrix4x4& mat) const
{
return HPoint(
x * mat.m[0][0] + y * mat.m[1][0] + z * mat.m[2][0] + w * mat.m[3][0],
x * mat.m[0][1] + y * mat.m[1][1] + z * mat.m[2][1] + w * mat.m[3][1],
x * mat.m[0][2] + y * mat.m[1][2] + z * mat.m[2][2] + w * mat.m[3][2],
x * mat.m[0][3] + y * mat.m[1][3] + z * mat.m[2][3] + w * mat.m[3][3]);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// HPoint *= Matrix4x4
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
HPoint& HPoint::operator*=(const Matrix4x4& mat)
{
float xp = x * mat.m[0][0] + y * mat.m[1][0] + z * mat.m[2][0] + w * mat.m[3][0];
float yp = x * mat.m[0][1] + y * mat.m[1][1] + z * mat.m[2][1] + w * mat.m[3][1];
float zp = x * mat.m[0][2] + y * mat.m[1][2] + z * mat.m[2][2] + w * mat.m[3][2];
float wp = x * mat.m[0][3] + y * mat.m[1][3] + z * mat.m[2][3] + w * mat.m[3][3];
x = xp; y = yp; z = zp; w = wp;
return *this;
}

160
ode/OPCODE/Ice/IceHPoint.h Normal file
View File

@ -0,0 +1,160 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for homogeneous points.
* \file IceHPoint.h
* \author Pierre Terdiman
* \date April, 4, 2000
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __ICEHPOINT_H__
#define __ICEHPOINT_H__
class ICEMATHS_API HPoint : public Point
{
public:
//! Empty constructor
inline_ HPoint() {}
//! Constructor from floats
inline_ HPoint(float xx, float yy, float zz, float ww=0.0f) : Point(xx, yy, zz), w(ww) {}
//! Constructor from array
inline_ HPoint(const float f[4]) : Point(f), w(f[3]) {}
//! Constructor from a Point
inline_ HPoint(const Point& p, float ww=0.0f) : Point(p), w(ww) {}
//! Destructor
inline_ ~HPoint() {}
//! Clear the point
inline_ HPoint& Zero() { x = y = z = w = 0.0f; return *this; }
//! Assignment from values
inline_ HPoint& Set(float xx, float yy, float zz, float ww ) { x = xx; y = yy; z = zz; w = ww; return *this; }
//! Assignment from array
inline_ HPoint& Set(const float f[4]) { x = f[X]; y = f[Y]; z = f[Z]; w = f[W]; return *this; }
//! Assignment from another h-point
inline_ HPoint& Set(const HPoint& src) { x = src.x; y = src.y; z = src.z; w = src.w; return *this; }
//! Add a vector
inline_ HPoint& Add(float xx, float yy, float zz, float ww ) { x += xx; y += yy; z += zz; w += ww; return *this; }
//! Add a vector
inline_ HPoint& Add(const float f[4]) { x += f[X]; y += f[Y]; z += f[Z]; w += f[W]; return *this; }
//! Subtract a vector
inline_ HPoint& Sub(float xx, float yy, float zz, float ww ) { x -= xx; y -= yy; z -= zz; w -= ww; return *this; }
//! Subtract a vector
inline_ HPoint& Sub(const float f[4]) { x -= f[X]; y -= f[Y]; z -= f[Z]; w -= f[W]; return *this; }
//! Multiplies by a scalar
inline_ HPoint& Mul(float s) { x *= s; y *= s; z *= s; w *= s; return *this; }
//! Returns MIN(x, y, z, w);
float Min() const { return MIN(x, MIN(y, MIN(z, w))); }
//! Returns MAX(x, y, z, w);
float Max() const { return MAX(x, MAX(y, MAX(z, w))); }
//! Sets each element to be componentwise minimum
HPoint& Min(const HPoint& p) { x = MIN(x, p.x); y = MIN(y, p.y); z = MIN(z, p.z); w = MIN(w, p.w); return *this; }
//! Sets each element to be componentwise maximum
HPoint& Max(const HPoint& p) { x = MAX(x, p.x); y = MAX(y, p.y); z = MAX(z, p.z); w = MAX(w, p.w); return *this; }
//! Computes square magnitude
inline_ float SquareMagnitude() const { return x*x + y*y + z*z + w*w; }
//! Computes magnitude
inline_ float Magnitude() const { return sqrtf(x*x + y*y + z*z + w*w); }
//! Normalize the vector
inline_ HPoint& Normalize()
{
float M = Magnitude();
if(M)
{
M = 1.0f / M;
x *= M;
y *= M;
z *= M;
w *= M;
}
return *this;
}
// Arithmetic operators
//! Operator for HPoint Negate = - HPoint;
inline_ HPoint operator-() const { return HPoint(-x, -y, -z, -w); }
//! Operator for HPoint Plus = HPoint + HPoint;
inline_ HPoint operator+(const HPoint& p) const { return HPoint(x + p.x, y + p.y, z + p.z, w + p.w); }
//! Operator for HPoint Minus = HPoint - HPoint;
inline_ HPoint operator-(const HPoint& p) const { return HPoint(x - p.x, y - p.y, z - p.z, w - p.w); }
//! Operator for HPoint Mul = HPoint * HPoint;
inline_ HPoint operator*(const HPoint& p) const { return HPoint(x * p.x, y * p.y, z * p.z, w * p.w); }
//! Operator for HPoint Scale = HPoint * float;
inline_ HPoint operator*(float s) const { return HPoint(x * s, y * s, z * s, w * s); }
//! Operator for HPoint Scale = float * HPoint;
inline_ friend HPoint operator*(float s, const HPoint& p) { return HPoint(s * p.x, s * p.y, s * p.z, s * p.w); }
//! Operator for HPoint Div = HPoint / HPoint;
inline_ HPoint operator/(const HPoint& p) const { return HPoint(x / p.x, y / p.y, z / p.z, w / p.w); }
//! Operator for HPoint Scale = HPoint / float;
inline_ HPoint operator/(float s) const { s = 1.0f / s; return HPoint(x * s, y * s, z * s, w * s); }
//! Operator for HPoint Scale = float / HPoint;
inline_ friend HPoint operator/(float s, const HPoint& p) { return HPoint(s / p.x, s / p.y, s / p.z, s / p.w); }
//! Operator for float DotProd = HPoint | HPoint;
inline_ float operator|(const HPoint& p) const { return x*p.x + y*p.y + z*p.z + w*p.w; }
// No cross-product in 4D
//! Operator for HPoint += HPoint;
inline_ HPoint& operator+=(const HPoint& p) { x += p.x; y += p.y; z += p.z; w += p.w; return *this; }
//! Operator for HPoint += float;
inline_ HPoint& operator+=(float s) { x += s; y += s; z += s; w += s; return *this; }
//! Operator for HPoint -= HPoint;
inline_ HPoint& operator-=(const HPoint& p) { x -= p.x; y -= p.y; z -= p.z; w -= p.w; return *this; }
//! Operator for HPoint -= float;
inline_ HPoint& operator-=(float s) { x -= s; y -= s; z -= s; w -= s; return *this; }
//! Operator for HPoint *= HPoint;
inline_ HPoint& operator*=(const HPoint& p) { x *= p.x; y *= p.y; z *= p.z; w *= p.w; return *this; }
//! Operator for HPoint *= float;
inline_ HPoint& operator*=(float s) { x*=s; y*=s; z*=s; w*=s; return *this; }
//! Operator for HPoint /= HPoint;
inline_ HPoint& operator/=(const HPoint& p) { x /= p.x; y /= p.y; z /= p.z; w /= p.w; return *this; }
//! Operator for HPoint /= float;
inline_ HPoint& operator/=(float s) { s = 1.0f / s; x*=s; y*=s; z*=s; w*=s; return *this; }
// Arithmetic operators
//! Operator for Point Mul = HPoint * Matrix3x3;
Point operator*(const Matrix3x3& mat) const;
//! Operator for HPoint Mul = HPoint * Matrix4x4;
HPoint operator*(const Matrix4x4& mat) const;
// HPoint *= Matrix3x3 doesn't exist, the matrix is first casted to a 4x4
//! Operator for HPoint *= Matrix4x4
HPoint& operator*=(const Matrix4x4& mat);
// Logical operators
//! Operator for "if(HPoint==HPoint)"
inline_ bool operator==(const HPoint& p) const { return ( (x==p.x)&&(y==p.y)&&(z==p.z)&&(w==p.w)); }
//! Operator for "if(HPoint!=HPoint)"
inline_ bool operator!=(const HPoint& p) const { return ( (x!=p.x)||(y!=p.y)||(z!=p.z)||(w!=p.w)); }
// Cast operators
//! Cast a HPoint to a Point. w is discarded.
#ifdef _MSC_VER
inline_ operator Point() const { return Point(x, y, z); }
// gcc complains that conversion to a base class will never use a type conversion operator
#endif
public:
float w;
};
#endif // __ICEHPOINT_H__

548
ode/OPCODE/Ice/IceIndexedTriangle.cpp Normal file
View File

@ -0,0 +1,548 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains a handy indexed triangle class.
* \file IceIndexedTriangle.cpp
* \author Pierre Terdiman
* \date January, 17, 2000
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Precompiled Header
#include "Stdafx.h"
using namespace IceMaths;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains an indexed triangle class.
*
* \class Triangle
* \author Pierre Terdiman
* \version 1.0
* \date 08.15.98
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Flips the winding order.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void IndexedTriangle::Flip()
{
Swap(mVRef[1], mVRef[2]);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the triangle area.
* \param verts [in] the list of indexed vertices
* \return the area
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
float IndexedTriangle::Area(const Point* verts) const
{
if(!verts) return 0.0f;
const Point& p0 = verts[0];
const Point& p1 = verts[1];
const Point& p2 = verts[2];
return ((p0-p1)^(p0-p2)).Magnitude() * 0.5f;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the triangle perimeter.
* \param verts [in] the list of indexed vertices
* \return the perimeter
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
float IndexedTriangle::Perimeter(const Point* verts) const
{
if(!verts) return 0.0f;
const Point& p0 = verts[0];
const Point& p1 = verts[1];
const Point& p2 = verts[2];
return p0.Distance(p1)
+ p0.Distance(p2)
+ p1.Distance(p2);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the triangle compacity.
* \param verts [in] the list of indexed vertices
* \return the compacity
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
float IndexedTriangle::Compacity(const Point* verts) const
{
if(!verts) return 0.0f;
float P = Perimeter(verts);
if(P==0.0f) return 0.0f;
return (4.0f*PI*Area(verts)/(P*P));
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the triangle normal.
* \param verts [in] the list of indexed vertices
* \param normal [out] the computed normal
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void IndexedTriangle::Normal(const Point* verts, Point& normal) const
{
if(!verts) return;
const Point& p0 = verts[mVRef[0]];
const Point& p1 = verts[mVRef[1]];
const Point& p2 = verts[mVRef[2]];
normal = ((p2-p1)^(p0-p1)).Normalize();
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the triangle denormalized normal.
* \param verts [in] the list of indexed vertices
* \param normal [out] the computed normal
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void IndexedTriangle::DenormalizedNormal(const Point* verts, Point& normal) const
{
if(!verts) return;
const Point& p0 = verts[mVRef[0]];
const Point& p1 = verts[mVRef[1]];
const Point& p2 = verts[mVRef[2]];
normal = ((p2-p1)^(p0-p1));
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the triangle center.
* \param verts [in] the list of indexed vertices
* \param center [out] the computed center
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void IndexedTriangle::Center(const Point* verts, Point& center) const
{
if(!verts) return;
const Point& p0 = verts[mVRef[0]];
const Point& p1 = verts[mVRef[1]];
const Point& p2 = verts[mVRef[2]];
center = (p0+p1+p2)*INV3;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the centered normal
* \param verts [in] the list of indexed vertices
* \param normal [out] the computed centered normal
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void IndexedTriangle::CenteredNormal(const Point* verts, Point& normal) const
{
if(!verts) return;
const Point& p0 = verts[mVRef[0]];
const Point& p1 = verts[mVRef[1]];
const Point& p2 = verts[mVRef[2]];
Point Center = (p0+p1+p2)*INV3;
normal = Center + ((p2-p1)^(p0-p1)).Normalize();
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes a random point within the triangle.
* \param verts [in] the list of indexed vertices
* \param normal [out] the computed centered normal
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void IndexedTriangle::RandomPoint(const Point* verts, Point& random) const
{
if(!verts) return;
// Random barycentric coords
float Alpha = UnitRandomFloat();
float Beta = UnitRandomFloat();
float Gamma = UnitRandomFloat();
float OneOverTotal = 1.0f / (Alpha + Beta + Gamma);
Alpha *= OneOverTotal;
Beta *= OneOverTotal;
Gamma *= OneOverTotal;
const Point& p0 = verts[mVRef[0]];
const Point& p1 = verts[mVRef[1]];
const Point& p2 = verts[mVRef[2]];
random = Alpha*p0 + Beta*p1 + Gamma*p2;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes backface culling.
* \param verts [in] the list of indexed vertices
* \param source [in] source point (in local space) from which culling must be computed
* \return true if the triangle is visible from the source point
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool IndexedTriangle::IsVisible(const Point* verts, const Point& source) const
{
// Checkings
if(!verts) return false;
const Point& p0 = verts[mVRef[0]];
const Point& p1 = verts[mVRef[1]];
const Point& p2 = verts[mVRef[2]];
// Compute denormalized normal
Point Normal = (p2 - p1)^(p0 - p1);
// Backface culling
return (Normal | source) >= 0.0f;
// Same as:
// Plane PL(verts[mVRef[0]], verts[mVRef[1]], verts[mVRef[2]]);
// return PL.Distance(source) > PL.d;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes backface culling.
* \param verts [in] the list of indexed vertices
* \param source [in] source point (in local space) from which culling must be computed
* \return true if the triangle is visible from the source point
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool IndexedTriangle::BackfaceCulling(const Point* verts, const Point& source) const
{
// Checkings
if(!verts) return false;
const Point& p0 = verts[mVRef[0]];
const Point& p1 = verts[mVRef[1]];
const Point& p2 = verts[mVRef[2]];
// Compute base
// Point Base = (p0 + p1 + p2)*INV3;
// Compute denormalized normal
Point Normal = (p2 - p1)^(p0 - p1);
// Backface culling
// return (Normal | (source - Base)) >= 0.0f;
return (Normal | (source - p0)) >= 0.0f;
// Same as: (but a bit faster)
// Plane PL(verts[mVRef[0]], verts[mVRef[1]], verts[mVRef[2]]);
// return PL.Distance(source)>0.0f;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the occlusion potential of the triangle.
* \param verts [in] the list of indexed vertices
* \param source [in] source point (in local space) from which occlusion potential must be computed
* \return the occlusion potential
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
float IndexedTriangle::ComputeOcclusionPotential(const Point* verts, const Point& view) const
{
if(!verts) return 0.0f;
// Occlusion potential: -(A * (N|V) / d^2)
// A = polygon area
// N = polygon normal
// V = view vector
// d = distance viewpoint-center of polygon
float A = Area(verts);
Point N; Normal(verts, N);
Point C; Center(verts, C);
float d = view.Distance(C);
return -(A*(N|view))/(d*d);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Replaces a vertex reference with another one.
* \param oldref [in] the vertex reference to replace
* \param newref [in] the new vertex reference
* \return true if success, else false if the input vertex reference doesn't belong to the triangle
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool IndexedTriangle::ReplaceVertex(udword oldref, udword newref)
{
if(mVRef[0]==oldref) { mVRef[0] = newref; return true; }
else if(mVRef[1]==oldref) { mVRef[1] = newref; return true; }
else if(mVRef[2]==oldref) { mVRef[2] = newref; return true; }
return false;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Checks whether the triangle is degenerate or not. A degenerate triangle has two common vertex references. This is a zero-area triangle.
* \return true if the triangle is degenerate
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool IndexedTriangle::IsDegenerate() const
{
if(mVRef[0]==mVRef[1]) return true;
if(mVRef[1]==mVRef[2]) return true;
if(mVRef[2]==mVRef[0]) return true;
return false;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Checks whether the input vertex reference belongs to the triangle or not.
* \param ref [in] the vertex reference to look for
* \return true if the triangle contains the vertex reference
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool IndexedTriangle::HasVertex(udword ref) const
{
if(mVRef[0]==ref) return true;
if(mVRef[1]==ref) return true;
if(mVRef[2]==ref) return true;
return false;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Checks whether the input vertex reference belongs to the triangle or not.
* \param ref [in] the vertex reference to look for
* \param index [out] the corresponding index in the triangle
* \return true if the triangle contains the vertex reference
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool IndexedTriangle::HasVertex(udword ref, udword* index) const
{
if(mVRef[0]==ref) { *index = 0; return true; }
if(mVRef[1]==ref) { *index = 1; return true; }
if(mVRef[2]==ref) { *index = 2; return true; }
return false;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Finds an edge in a tri, given two vertex references.
* \param vref0 [in] the edge's first vertex reference
* \param vref1 [in] the edge's second vertex reference
* \return the edge number between 0 and 2, or 0xff if input refs are wrong.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ubyte IndexedTriangle::FindEdge(udword vref0, udword vref1) const
{
if(mVRef[0]==vref0 && mVRef[1]==vref1) return 0;
else if(mVRef[0]==vref1 && mVRef[1]==vref0) return 0;
else if(mVRef[0]==vref0 && mVRef[2]==vref1) return 1;
else if(mVRef[0]==vref1 && mVRef[2]==vref0) return 1;
else if(mVRef[1]==vref0 && mVRef[2]==vref1) return 2;
else if(mVRef[1]==vref1 && mVRef[2]==vref0) return 2;
return 0xff;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Gets the last reference given the first two.
* \param vref0 [in] the first vertex reference
* \param vref1 [in] the second vertex reference
* \return the last reference, or INVALID_ID if input refs are wrong.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
udword IndexedTriangle::OppositeVertex(udword vref0, udword vref1) const
{
if(mVRef[0]==vref0 && mVRef[1]==vref1) return mVRef[2];
else if(mVRef[0]==vref1 && mVRef[1]==vref0) return mVRef[2];
else if(mVRef[0]==vref0 && mVRef[2]==vref1) return mVRef[1];
else if(mVRef[0]==vref1 && mVRef[2]==vref0) return mVRef[1];
else if(mVRef[1]==vref0 && mVRef[2]==vref1) return mVRef[0];
else if(mVRef[1]==vref1 && mVRef[2]==vref0) return mVRef[0];
return INVALID_ID;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Gets the three sorted vertex references according to an edge number.
* edgenb = 0 => edge 0-1, returns references 0, 1, 2
* edgenb = 1 => edge 0-2, returns references 0, 2, 1
* edgenb = 2 => edge 1-2, returns references 1, 2, 0
*
* \param edgenb [in] the edge number, 0, 1 or 2
* \param vref0 [out] the returned first vertex reference
* \param vref1 [out] the returned second vertex reference
* \param vref2 [out] the returned third vertex reference
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void IndexedTriangle::GetVRefs(ubyte edgenb, udword& vref0, udword& vref1, udword& vref2) const
{
if(edgenb==0)
{
vref0 = mVRef[0];
vref1 = mVRef[1];
vref2 = mVRef[2];
}
else if(edgenb==1)
{
vref0 = mVRef[0];
vref1 = mVRef[2];
vref2 = mVRef[1];
}
else if(edgenb==2)
{
vref0 = mVRef[1];
vref1 = mVRef[2];
vref2 = mVRef[0];
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the triangle's smallest edge length.
* \param verts [in] the list of indexed vertices
* \return the smallest edge length
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
float IndexedTriangle::MinEdgeLength(const Point* verts) const
{
if(!verts) return 0.0f;
float Min = MAX_FLOAT;
float Length01 = verts[0].Distance(verts[1]);
float Length02 = verts[0].Distance(verts[2]);
float Length12 = verts[1].Distance(verts[2]);
if(Length01 < Min) Min = Length01;
if(Length02 < Min) Min = Length02;
if(Length12 < Min) Min = Length12;
return Min;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the triangle's largest edge length.
* \param verts [in] the list of indexed vertices
* \return the largest edge length
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
float IndexedTriangle::MaxEdgeLength(const Point* verts) const
{
if(!verts) return 0.0f;
float Max = MIN_FLOAT;
float Length01 = verts[0].Distance(verts[1]);
float Length02 = verts[0].Distance(verts[2]);
float Length12 = verts[1].Distance(verts[2]);
if(Length01 > Max) Max = Length01;
if(Length02 > Max) Max = Length02;
if(Length12 > Max) Max = Length12;
return Max;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes a point on the triangle according to the stabbing information.
* \param verts [in] the list of indexed vertices
* \param u,v [in] point's barycentric coordinates
* \param pt [out] point on triangle
* \param nearvtx [out] index of nearest vertex
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void IndexedTriangle::ComputePoint(const Point* verts, float u, float v, Point& pt, udword* nearvtx) const
{
// Checkings
if(!verts) return;
// Get face in local or global space
const Point& p0 = verts[mVRef[0]];
const Point& p1 = verts[mVRef[1]];
const Point& p2 = verts[mVRef[2]];
// Compute point coordinates
pt = (1.0f - u - v)*p0 + u*p1 + v*p2;
// Compute nearest vertex if needed
if(nearvtx)
{
// Compute distance vector
Point d(p0.SquareDistance(pt), // Distance^2 from vertex 0 to point on the face
p1.SquareDistance(pt), // Distance^2 from vertex 1 to point on the face
p2.SquareDistance(pt)); // Distance^2 from vertex 2 to point on the face
// Get smallest distance
*nearvtx = mVRef[d.SmallestAxis()];
}
}
//**************************************
// Angle between two vectors (in radians)
// we use this formula
// uv = |u||v| cos(u,v)
// u ^ v = w
// |w| = |u||v| |sin(u,v)|
//**************************************
float Angle(const Point& u, const Point& v)
{
float NormU = u.Magnitude(); // |u|
float NormV = v.Magnitude(); // |v|
float Product = NormU*NormV; // |u||v|
if(Product==0.0f) return 0.0f;
float OneOverProduct = 1.0f / Product;
// Cosinus
float Cosinus = (u|v) * OneOverProduct;
// Sinus
Point w = u^v;
float NormW = w.Magnitude();
float AbsSinus = NormW * OneOverProduct;
// Remove degeneracy
if(AbsSinus > 1.0f) AbsSinus = 1.0f;
if(AbsSinus < -1.0f) AbsSinus = -1.0f;
if(Cosinus>=0.0f) return asinf(AbsSinus);
else return (PI-asinf(AbsSinus));
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the angle between two triangles.
* \param tri [in] the other triangle
* \param verts [in] the list of indexed vertices
* \return the angle in radians
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
float IndexedTriangle::Angle(const IndexedTriangle& tri, const Point* verts) const
{
// Checkings
if(!verts) return 0.0f;
// Compute face normals
Point n0, n1;
Normal(verts, n0);
tri.Normal(verts, n1);
// Compute angle
float dp = n0|n1;
if(dp>1.0f) return 0.0f;
if(dp<-1.0f) return PI;
return acosf(dp);
// return ::Angle(n0,n1);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Checks a triangle is the same as another one.
* \param tri [in] the other triangle
* \return true if same triangle
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool IndexedTriangle::Equal(const IndexedTriangle& tri) const
{
// Test all vertex references
return (HasVertex(tri.mVRef[0]) &&
HasVertex(tri.mVRef[1]) &&
HasVertex(tri.mVRef[2]));
}

72
ode/OPCODE/Ice/IceIndexedTriangle.h Normal file
View File

@ -0,0 +1,72 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains a handy indexed triangle class.
* \file IceIndexedTriangle.h
* \author Pierre Terdiman
* \date January, 17, 2000
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __ICEINDEXEDTRIANGLE_H__
#define __ICEINDEXEDTRIANGLE_H__
// Forward declarations
#ifdef _MSC_VER
enum CubeIndex;
#else
typedef int CubeIndex;
#endif
// An indexed triangle class.
class ICEMATHS_API IndexedTriangle
{
public:
//! Constructor
inline_ IndexedTriangle() {}
//! Constructor
inline_ IndexedTriangle(udword r0, udword r1, udword r2) { mVRef[0]=r0; mVRef[1]=r1; mVRef[2]=r2; }
//! Copy constructor
inline_ IndexedTriangle(const IndexedTriangle& triangle)
{
mVRef[0] = triangle.mVRef[0];
mVRef[1] = triangle.mVRef[1];
mVRef[2] = triangle.mVRef[2];
}
//! Destructor
inline_ ~IndexedTriangle() {}
//! Vertex-references
udword mVRef[3];
// Methods
void Flip();
float Area(const Point* verts) const;
float Perimeter(const Point* verts) const;
float Compacity(const Point* verts) const;
void Normal(const Point* verts, Point& normal) const;
void DenormalizedNormal(const Point* verts, Point& normal) const;
void Center(const Point* verts, Point& center) const;
void CenteredNormal(const Point* verts, Point& normal) const;
void RandomPoint(const Point* verts, Point& random) const;
bool IsVisible(const Point* verts, const Point& source) const;
bool BackfaceCulling(const Point* verts, const Point& source) const;
float ComputeOcclusionPotential(const Point* verts, const Point& view) const;
bool ReplaceVertex(udword oldref, udword newref);
bool IsDegenerate() const;
bool HasVertex(udword ref) const;
bool HasVertex(udword ref, udword* index) const;
ubyte FindEdge(udword vref0, udword vref1) const;
udword OppositeVertex(udword vref0, udword vref1) const;
inline_ udword OppositeVertex(ubyte edgenb) const { return mVRef[2-edgenb]; }
void GetVRefs(ubyte edgenb, udword& vref0, udword& vref1, udword& vref2) const;
float MinEdgeLength(const Point* verts) const;
float MaxEdgeLength(const Point* verts) const;
void ComputePoint(const Point* verts, float u, float v, Point& pt, udword* nearvtx=null) const;
float Angle(const IndexedTriangle& tri, const Point* verts) const;
inline_ Plane PlaneEquation(const Point* verts) const { return Plane(verts[mVRef[0]], verts[mVRef[1]], verts[mVRef[2]]); }
bool Equal(const IndexedTriangle& tri) const;
CubeIndex ComputeCubeIndex(const Point* verts) const;
};
#endif // __ICEINDEXEDTRIANGLE_H__

75
ode/OPCODE/Ice/IceLSS.h Normal file
View File

@ -0,0 +1,75 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for line-swept spheres.
* \file IceLSS.h
* \author Pierre Terdiman
* \date April, 4, 2000
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __ICELSS_H__
#define __ICELSS_H__
class ICEMATHS_API LSS : public Segment
{
public:
//! Constructor
inline_ LSS() {}
//! Constructor
inline_ LSS(const Segment& seg, float radius) : Segment(seg), mRadius(radius) {}
//! Destructor
inline_ ~LSS() {}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes an OBB surrounding the LSS.
* \param box [out] the OBB
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void ComputeOBB(OBB& box);
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Tests if a point is contained within the LSS.
* \param pt [in] the point to test
* \return true if inside the LSS
* \warning point and LSS must be in same space
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ bool Contains(const Point& pt) const { return SquareDistance(pt) <= mRadius*mRadius; }
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Tests if a sphere is contained within the LSS.
* \param sphere [in] the sphere to test
* \return true if inside the LSS
* \warning sphere and LSS must be in same space
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ bool Contains(const Sphere& sphere)
{
float d = mRadius - sphere.mRadius;
if(d>=0.0f) return SquareDistance(sphere.mCenter) <= d*d;
else return false;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Tests if an LSS is contained within the LSS.
* \param lss [in] the LSS to test
* \return true if inside the LSS
* \warning both LSS must be in same space
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ bool Contains(const LSS& lss)
{
// We check the LSS contains the two spheres at the start and end of the sweep
return Contains(Sphere(lss.mP0, lss.mRadius)) && Contains(Sphere(lss.mP0, lss.mRadius));
}
float mRadius; //!< Sphere radius
};
#endif // __ICELSS_H__

48
ode/OPCODE/Ice/IceMatrix3x3.cpp Normal file
View File

@ -0,0 +1,48 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for 3x3 matrices.
* \file IceMatrix3x3.cpp
* \author Pierre Terdiman
* \date April, 4, 2000
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* 3x3 matrix.
* DirectX-compliant, ie row-column order, ie m[Row][Col].
* Same as:
* m11 m12 m13 first row.
* m21 m22 m23 second row.
* m31 m32 m33 third row.
* Stored in memory as m11 m12 m13 m21...
*
* Multiplication rules:
*
* [x'y'z'] = [xyz][M]
*
* x' = x*m11 + y*m21 + z*m31
* y' = x*m12 + y*m22 + z*m32
* z' = x*m13 + y*m23 + z*m33
*
* \class Matrix3x3
* \author Pierre Terdiman
* \version 1.0
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Precompiled Header
#include "Stdafx.h"
using namespace IceMaths;
// Cast operator
Matrix3x3::operator Matrix4x4() const
{
return Matrix4x4(
m[0][0], m[0][1], m[0][2], 0.0f,
m[1][0], m[1][1], m[1][2], 0.0f,
m[2][0], m[2][1], m[2][2], 0.0f,
0.0f, 0.0f, 0.0f, 1.0f);
}

496
ode/OPCODE/Ice/IceMatrix3x3.h Normal file
View File

@ -0,0 +1,496 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for 3x3 matrices.
* \file IceMatrix3x3.h
* \author Pierre Terdiman
* \date April, 4, 2000
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __ICEMATRIX3X3_H__
#define __ICEMATRIX3X3_H__
// Forward declarations
class Quat;
#define MATRIX3X3_EPSILON (1.0e-7f)
class ICEMATHS_API Matrix3x3
{
public:
//! Empty constructor
inline_ Matrix3x3() {}
//! Constructor from 9 values
inline_ Matrix3x3(float m00, float m01, float m02, float m10, float m11, float m12, float m20, float m21, float m22)
{
m[0][0] = m00; m[0][1] = m01; m[0][2] = m02;
m[1][0] = m10; m[1][1] = m11; m[1][2] = m12;
m[2][0] = m20; m[2][1] = m21; m[2][2] = m22;
}
//! Copy constructor
inline_ Matrix3x3(const Matrix3x3& mat) { CopyMemory(m, &mat.m, 9*sizeof(float)); }
//! Destructor
inline_ ~Matrix3x3() {}
//! Assign values
inline_ void Set(float m00, float m01, float m02, float m10, float m11, float m12, float m20, float m21, float m22)
{
m[0][0] = m00; m[0][1] = m01; m[0][2] = m02;
m[1][0] = m10; m[1][1] = m11; m[1][2] = m12;
m[2][0] = m20; m[2][1] = m21; m[2][2] = m22;
}
//! Sets the scale from a Point. The point is put on the diagonal.
inline_ void SetScale(const Point& p) { m[0][0] = p.x; m[1][1] = p.y; m[2][2] = p.z; }
//! Sets the scale from floats. Values are put on the diagonal.
inline_ void SetScale(float sx, float sy, float sz) { m[0][0] = sx; m[1][1] = sy; m[2][2] = sz; }
//! Scales from a Point. Each row is multiplied by a component.
inline_ void Scale(const Point& p)
{
m[0][0] *= p.x; m[0][1] *= p.x; m[0][2] *= p.x;
m[1][0] *= p.y; m[1][1] *= p.y; m[1][2] *= p.y;
m[2][0] *= p.z; m[2][1] *= p.z; m[2][2] *= p.z;
}
//! Scales from floats. Each row is multiplied by a value.
inline_ void Scale(float sx, float sy, float sz)
{
m[0][0] *= sx; m[0][1] *= sx; m[0][2] *= sx;
m[1][0] *= sy; m[1][1] *= sy; m[1][2] *= sy;
m[2][0] *= sz; m[2][1] *= sz; m[2][2] *= sz;
}
//! Copy from a Matrix3x3
inline_ void Copy(const Matrix3x3& source) { CopyMemory(m, source.m, 9*sizeof(float)); }
// Row-column access
//! Returns a row.
inline_ void GetRow(const udword r, Point& p) const { p.x = m[r][0]; p.y = m[r][1]; p.z = m[r][2]; }
//! Returns a row.
inline_ const Point& GetRow(const udword r) const { return *(const Point*)&m[r][0]; }
//! Returns a row.
inline_ Point& GetRow(const udword r) { return *(Point*)&m[r][0]; }
//! Sets a row.
inline_ void SetRow(const udword r, const Point& p) { m[r][0] = p.x; m[r][1] = p.y; m[r][2] = p.z; }
//! Returns a column.
inline_ void GetCol(const udword c, Point& p) const { p.x = m[0][c]; p.y = m[1][c]; p.z = m[2][c]; }
//! Sets a column.
inline_ void SetCol(const udword c, const Point& p) { m[0][c] = p.x; m[1][c] = p.y; m[2][c] = p.z; }
//! Computes the trace. The trace is the sum of the 3 diagonal components.
inline_ float Trace() const { return m[0][0] + m[1][1] + m[2][2]; }
//! Clears the matrix.
inline_ void Zero() { ZeroMemory(&m, sizeof(m)); }
//! Sets the identity matrix.
inline_ void Identity() { Zero(); m[0][0] = m[1][1] = m[2][2] = 1.0f; }
//! Checks for identity
inline_ bool IsIdentity() const
{
if(IR(m[0][0])!=IEEE_1_0) return false;
if(IR(m[0][1])!=0) return false;
if(IR(m[0][2])!=0) return false;
if(IR(m[1][0])!=0) return false;
if(IR(m[1][1])!=IEEE_1_0) return false;
if(IR(m[1][2])!=0) return false;
if(IR(m[2][0])!=0) return false;
if(IR(m[2][1])!=0) return false;
if(IR(m[2][2])!=IEEE_1_0) return false;
return true;
}
//! Checks matrix validity
inline_ BOOL IsValid() const
{
for(udword j=0;j<3;j++)
{
for(udword i=0;i<3;i++)
{
if(!IsValidFloat(m[j][i])) return FALSE;
}
}
return TRUE;
}
//! Makes a skew-symmetric matrix (a.k.a. Star(*) Matrix)
//! [ 0.0 -a.z a.y ]
//! [ a.z 0.0 -a.x ]
//! [ -a.y a.x 0.0 ]
//! This is also called a "cross matrix" since for any vectors A and B,
//! A^B = Skew(A) * B = - B * Skew(A);
inline_ void SkewSymmetric(const Point& a)
{
m[0][0] = 0.0f;
m[0][1] = -a.z;
m[0][2] = a.y;
m[1][0] = a.z;
m[1][1] = 0.0f;
m[1][2] = -a.x;
m[2][0] = -a.y;
m[2][1] = a.x;
m[2][2] = 0.0f;
}
//! Negates the matrix
inline_ void Neg()
{
m[0][0] = -m[0][0]; m[0][1] = -m[0][1]; m[0][2] = -m[0][2];
m[1][0] = -m[1][0]; m[1][1] = -m[1][1]; m[1][2] = -m[1][2];
m[2][0] = -m[2][0]; m[2][1] = -m[2][1]; m[2][2] = -m[2][2];
}
//! Neg from another matrix
inline_ void Neg(const Matrix3x3& mat)
{
m[0][0] = -mat.m[0][0]; m[0][1] = -mat.m[0][1]; m[0][2] = -mat.m[0][2];
m[1][0] = -mat.m[1][0]; m[1][1] = -mat.m[1][1]; m[1][2] = -mat.m[1][2];
m[2][0] = -mat.m[2][0]; m[2][1] = -mat.m[2][1]; m[2][2] = -mat.m[2][2];
}
//! Add another matrix
inline_ void Add(const Matrix3x3& mat)
{
m[0][0] += mat.m[0][0]; m[0][1] += mat.m[0][1]; m[0][2] += mat.m[0][2];
m[1][0] += mat.m[1][0]; m[1][1] += mat.m[1][1]; m[1][2] += mat.m[1][2];
m[2][0] += mat.m[2][0]; m[2][1] += mat.m[2][1]; m[2][2] += mat.m[2][2];
}
//! Sub another matrix
inline_ void Sub(const Matrix3x3& mat)
{
m[0][0] -= mat.m[0][0]; m[0][1] -= mat.m[0][1]; m[0][2] -= mat.m[0][2];
m[1][0] -= mat.m[1][0]; m[1][1] -= mat.m[1][1]; m[1][2] -= mat.m[1][2];
m[2][0] -= mat.m[2][0]; m[2][1] -= mat.m[2][1]; m[2][2] -= mat.m[2][2];
}
//! Mac
inline_ void Mac(const Matrix3x3& a, const Matrix3x3& b, float s)
{
m[0][0] = a.m[0][0] + b.m[0][0] * s;
m[0][1] = a.m[0][1] + b.m[0][1] * s;
m[0][2] = a.m[0][2] + b.m[0][2] * s;
m[1][0] = a.m[1][0] + b.m[1][0] * s;
m[1][1] = a.m[1][1] + b.m[1][1] * s;
m[1][2] = a.m[1][2] + b.m[1][2] * s;
m[2][0] = a.m[2][0] + b.m[2][0] * s;
m[2][1] = a.m[2][1] + b.m[2][1] * s;
m[2][2] = a.m[2][2] + b.m[2][2] * s;
}
//! Mac
inline_ void Mac(const Matrix3x3& a, float s)
{
m[0][0] += a.m[0][0] * s; m[0][1] += a.m[0][1] * s; m[0][2] += a.m[0][2] * s;
m[1][0] += a.m[1][0] * s; m[1][1] += a.m[1][1] * s; m[1][2] += a.m[1][2] * s;
m[2][0] += a.m[2][0] * s; m[2][1] += a.m[2][1] * s; m[2][2] += a.m[2][2] * s;
}
//! this = A * s
inline_ void Mult(const Matrix3x3& a, float s)
{
m[0][0] = a.m[0][0] * s; m[0][1] = a.m[0][1] * s; m[0][2] = a.m[0][2] * s;
m[1][0] = a.m[1][0] * s; m[1][1] = a.m[1][1] * s; m[1][2] = a.m[1][2] * s;
m[2][0] = a.m[2][0] * s; m[2][1] = a.m[2][1] * s; m[2][2] = a.m[2][2] * s;
}
inline_ void Add(const Matrix3x3& a, const Matrix3x3& b)
{
m[0][0] = a.m[0][0] + b.m[0][0]; m[0][1] = a.m[0][1] + b.m[0][1]; m[0][2] = a.m[0][2] + b.m[0][2];
m[1][0] = a.m[1][0] + b.m[1][0]; m[1][1] = a.m[1][1] + b.m[1][1]; m[1][2] = a.m[1][2] + b.m[1][2];
m[2][0] = a.m[2][0] + b.m[2][0]; m[2][1] = a.m[2][1] + b.m[2][1]; m[2][2] = a.m[2][2] + b.m[2][2];
}
inline_ void Sub(const Matrix3x3& a, const Matrix3x3& b)
{
m[0][0] = a.m[0][0] - b.m[0][0]; m[0][1] = a.m[0][1] - b.m[0][1]; m[0][2] = a.m[0][2] - b.m[0][2];
m[1][0] = a.m[1][0] - b.m[1][0]; m[1][1] = a.m[1][1] - b.m[1][1]; m[1][2] = a.m[1][2] - b.m[1][2];
m[2][0] = a.m[2][0] - b.m[2][0]; m[2][1] = a.m[2][1] - b.m[2][1]; m[2][2] = a.m[2][2] - b.m[2][2];
}
//! this = a * b
inline_ void Mult(const Matrix3x3& a, const Matrix3x3& b)
{
m[0][0] = a.m[0][0] * b.m[0][0] + a.m[0][1] * b.m[1][0] + a.m[0][2] * b.m[2][0];
m[0][1] = a.m[0][0] * b.m[0][1] + a.m[0][1] * b.m[1][1] + a.m[0][2] * b.m[2][1];
m[0][2] = a.m[0][0] * b.m[0][2] + a.m[0][1] * b.m[1][2] + a.m[0][2] * b.m[2][2];
m[1][0] = a.m[1][0] * b.m[0][0] + a.m[1][1] * b.m[1][0] + a.m[1][2] * b.m[2][0];
m[1][1] = a.m[1][0] * b.m[0][1] + a.m[1][1] * b.m[1][1] + a.m[1][2] * b.m[2][1];
m[1][2] = a.m[1][0] * b.m[0][2] + a.m[1][1] * b.m[1][2] + a.m[1][2] * b.m[2][2];
m[2][0] = a.m[2][0] * b.m[0][0] + a.m[2][1] * b.m[1][0] + a.m[2][2] * b.m[2][0];
m[2][1] = a.m[2][0] * b.m[0][1] + a.m[2][1] * b.m[1][1] + a.m[2][2] * b.m[2][1];
m[2][2] = a.m[2][0] * b.m[0][2] + a.m[2][1] * b.m[1][2] + a.m[2][2] * b.m[2][2];
}
//! this = transpose(a) * b
inline_ void MultAtB(const Matrix3x3& a, const Matrix3x3& b)
{
m[0][0] = a.m[0][0] * b.m[0][0] + a.m[1][0] * b.m[1][0] + a.m[2][0] * b.m[2][0];
m[0][1] = a.m[0][0] * b.m[0][1] + a.m[1][0] * b.m[1][1] + a.m[2][0] * b.m[2][1];
m[0][2] = a.m[0][0] * b.m[0][2] + a.m[1][0] * b.m[1][2] + a.m[2][0] * b.m[2][2];
m[1][0] = a.m[0][1] * b.m[0][0] + a.m[1][1] * b.m[1][0] + a.m[2][1] * b.m[2][0];
m[1][1] = a.m[0][1] * b.m[0][1] + a.m[1][1] * b.m[1][1] + a.m[2][1] * b.m[2][1];
m[1][2] = a.m[0][1] * b.m[0][2] + a.m[1][1] * b.m[1][2] + a.m[2][1] * b.m[2][2];
m[2][0] = a.m[0][2] * b.m[0][0] + a.m[1][2] * b.m[1][0] + a.m[2][2] * b.m[2][0];
m[2][1] = a.m[0][2] * b.m[0][1] + a.m[1][2] * b.m[1][1] + a.m[2][2] * b.m[2][1];
m[2][2] = a.m[0][2] * b.m[0][2] + a.m[1][2] * b.m[1][2] + a.m[2][2] * b.m[2][2];
}
//! this = a * transpose(b)
inline_ void MultABt(const Matrix3x3& a, const Matrix3x3& b)
{
m[0][0] = a.m[0][0] * b.m[0][0] + a.m[0][1] * b.m[0][1] + a.m[0][2] * b.m[0][2];
m[0][1] = a.m[0][0] * b.m[1][0] + a.m[0][1] * b.m[1][1] + a.m[0][2] * b.m[1][2];
m[0][2] = a.m[0][0] * b.m[2][0] + a.m[0][1] * b.m[2][1] + a.m[0][2] * b.m[2][2];
m[1][0] = a.m[1][0] * b.m[0][0] + a.m[1][1] * b.m[0][1] + a.m[1][2] * b.m[0][2];
m[1][1] = a.m[1][0] * b.m[1][0] + a.m[1][1] * b.m[1][1] + a.m[1][2] * b.m[1][2];
m[1][2] = a.m[1][0] * b.m[2][0] + a.m[1][1] * b.m[2][1] + a.m[1][2] * b.m[2][2];
m[2][0] = a.m[2][0] * b.m[0][0] + a.m[2][1] * b.m[0][1] + a.m[2][2] * b.m[0][2];
m[2][1] = a.m[2][0] * b.m[1][0] + a.m[2][1] * b.m[1][1] + a.m[2][2] * b.m[1][2];
m[2][2] = a.m[2][0] * b.m[2][0] + a.m[2][1] * b.m[2][1] + a.m[2][2] * b.m[2][2];
}
//! Makes a rotation matrix mapping vector "from" to vector "to".
Matrix3x3& FromTo(const Point& from, const Point& to);
//! Set a rotation matrix around the X axis.
//! 1 0 0
//! RX = 0 cx sx
//! 0 -sx cx
void RotX(float angle);
//! Set a rotation matrix around the Y axis.
//! cy 0 -sy
//! RY = 0 1 0
//! sy 0 cy
void RotY(float angle);
//! Set a rotation matrix around the Z axis.
//! cz sz 0
//! RZ = -sz cz 0
//! 0 0 1
void RotZ(float angle);
//! cy sx.sy -sy.cx
//! RY.RX 0 cx sx
//! sy -sx.cy cx.cy
void RotYX(float y, float x);
//! Make a rotation matrix about an arbitrary axis
Matrix3x3& Rot(float angle, const Point& axis);
//! Transpose the matrix.
void Transpose()
{
IR(m[1][0]) ^= IR(m[0][1]); IR(m[0][1]) ^= IR(m[1][0]); IR(m[1][0]) ^= IR(m[0][1]);
IR(m[2][0]) ^= IR(m[0][2]); IR(m[0][2]) ^= IR(m[2][0]); IR(m[2][0]) ^= IR(m[0][2]);
IR(m[2][1]) ^= IR(m[1][2]); IR(m[1][2]) ^= IR(m[2][1]); IR(m[2][1]) ^= IR(m[1][2]);
}
//! this = Transpose(a)
void Transpose(const Matrix3x3& a)
{
m[0][0] = a.m[0][0]; m[0][1] = a.m[1][0]; m[0][2] = a.m[2][0];
m[1][0] = a.m[0][1]; m[1][1] = a.m[1][1]; m[1][2] = a.m[2][1];
m[2][0] = a.m[0][2]; m[2][1] = a.m[1][2]; m[2][2] = a.m[2][2];
}
//! Compute the determinant of the matrix. We use the rule of Sarrus.
float Determinant() const
{
return (m[0][0]*m[1][1]*m[2][2] + m[0][1]*m[1][2]*m[2][0] + m[0][2]*m[1][0]*m[2][1])
- (m[2][0]*m[1][1]*m[0][2] + m[2][1]*m[1][2]*m[0][0] + m[2][2]*m[1][0]*m[0][1]);
}
/*
//! Compute a cofactor. Used for matrix inversion.
float CoFactor(ubyte row, ubyte column) const
{
static sdword gIndex[3+2] = { 0, 1, 2, 0, 1 };
return (m[gIndex[row+1]][gIndex[column+1]]*m[gIndex[row+2]][gIndex[column+2]] - m[gIndex[row+2]][gIndex[column+1]]*m[gIndex[row+1]][gIndex[column+2]]);
}
*/
//! Invert the matrix. Determinant must be different from zero, else matrix can't be inverted.
Matrix3x3& Invert()
{
float Det = Determinant(); // Must be !=0
float OneOverDet = 1.0f / Det;
Matrix3x3 Temp;
Temp.m[0][0] = +(m[1][1] * m[2][2] - m[2][1] * m[1][2]) * OneOverDet;
Temp.m[1][0] = -(m[1][0] * m[2][2] - m[2][0] * m[1][2]) * OneOverDet;
Temp.m[2][0] = +(m[1][0] * m[2][1] - m[2][0] * m[1][1]) * OneOverDet;
Temp.m[0][1] = -(m[0][1] * m[2][2] - m[2][1] * m[0][2]) * OneOverDet;
Temp.m[1][1] = +(m[0][0] * m[2][2] - m[2][0] * m[0][2]) * OneOverDet;
Temp.m[2][1] = -(m[0][0] * m[2][1] - m[2][0] * m[0][1]) * OneOverDet;
Temp.m[0][2] = +(m[0][1] * m[1][2] - m[1][1] * m[0][2]) * OneOverDet;
Temp.m[1][2] = -(m[0][0] * m[1][2] - m[1][0] * m[0][2]) * OneOverDet;
Temp.m[2][2] = +(m[0][0] * m[1][1] - m[1][0] * m[0][1]) * OneOverDet;
*this = Temp;
return *this;
}
Matrix3x3& Normalize();
//! this = exp(a)
Matrix3x3& Exp(const Matrix3x3& a);
void FromQuat(const Quat &q);
void FromQuatL2(const Quat &q, float l2);
// Arithmetic operators
//! Operator for Matrix3x3 Plus = Matrix3x3 + Matrix3x3;
inline_ Matrix3x3 operator+(const Matrix3x3& mat) const
{
return Matrix3x3(
m[0][0] + mat.m[0][0], m[0][1] + mat.m[0][1], m[0][2] + mat.m[0][2],
m[1][0] + mat.m[1][0], m[1][1] + mat.m[1][1], m[1][2] + mat.m[1][2],
m[2][0] + mat.m[2][0], m[2][1] + mat.m[2][1], m[2][2] + mat.m[2][2]);
}
//! Operator for Matrix3x3 Minus = Matrix3x3 - Matrix3x3;
inline_ Matrix3x3 operator-(const Matrix3x3& mat) const
{
return Matrix3x3(
m[0][0] - mat.m[0][0], m[0][1] - mat.m[0][1], m[0][2] - mat.m[0][2],
m[1][0] - mat.m[1][0], m[1][1] - mat.m[1][1], m[1][2] - mat.m[1][2],
m[2][0] - mat.m[2][0], m[2][1] - mat.m[2][1], m[2][2] - mat.m[2][2]);
}
//! Operator for Matrix3x3 Mul = Matrix3x3 * Matrix3x3;
inline_ Matrix3x3 operator*(const Matrix3x3& mat) const
{
return Matrix3x3(
m[0][0]*mat.m[0][0] + m[0][1]*mat.m[1][0] + m[0][2]*mat.m[2][0],
m[0][0]*mat.m[0][1] + m[0][1]*mat.m[1][1] + m[0][2]*mat.m[2][1],
m[0][0]*mat.m[0][2] + m[0][1]*mat.m[1][2] + m[0][2]*mat.m[2][2],
m[1][0]*mat.m[0][0] + m[1][1]*mat.m[1][0] + m[1][2]*mat.m[2][0],
m[1][0]*mat.m[0][1] + m[1][1]*mat.m[1][1] + m[1][2]*mat.m[2][1],
m[1][0]*mat.m[0][2] + m[1][1]*mat.m[1][2] + m[1][2]*mat.m[2][2],
m[2][0]*mat.m[0][0] + m[2][1]*mat.m[1][0] + m[2][2]*mat.m[2][0],
m[2][0]*mat.m[0][1] + m[2][1]*mat.m[1][1] + m[2][2]*mat.m[2][1],
m[2][0]*mat.m[0][2] + m[2][1]*mat.m[1][2] + m[2][2]*mat.m[2][2]);
}
//! Operator for Point Mul = Matrix3x3 * Point;
inline_ Point operator*(const Point& v) const { return Point(GetRow(0)|v, GetRow(1)|v, GetRow(2)|v); }
//! Operator for Matrix3x3 Mul = Matrix3x3 * float;
inline_ Matrix3x3 operator*(float s) const
{
return Matrix3x3(
m[0][0]*s, m[0][1]*s, m[0][2]*s,
m[1][0]*s, m[1][1]*s, m[1][2]*s,
m[2][0]*s, m[2][1]*s, m[2][2]*s);
}
//! Operator for Matrix3x3 Mul = float * Matrix3x3;
inline_ friend Matrix3x3 operator*(float s, const Matrix3x3& mat)
{
return Matrix3x3(
s*mat.m[0][0], s*mat.m[0][1], s*mat.m[0][2],
s*mat.m[1][0], s*mat.m[1][1], s*mat.m[1][2],
s*mat.m[2][0], s*mat.m[2][1], s*mat.m[2][2]);
}
//! Operator for Matrix3x3 Div = Matrix3x3 / float;
inline_ Matrix3x3 operator/(float s) const
{
if (s) s = 1.0f / s;
return Matrix3x3(
m[0][0]*s, m[0][1]*s, m[0][2]*s,
m[1][0]*s, m[1][1]*s, m[1][2]*s,
m[2][0]*s, m[2][1]*s, m[2][2]*s);
}
//! Operator for Matrix3x3 Div = float / Matrix3x3;
inline_ friend Matrix3x3 operator/(float s, const Matrix3x3& mat)
{
return Matrix3x3(
s/mat.m[0][0], s/mat.m[0][1], s/mat.m[0][2],
s/mat.m[1][0], s/mat.m[1][1], s/mat.m[1][2],
s/mat.m[2][0], s/mat.m[2][1], s/mat.m[2][2]);
}
//! Operator for Matrix3x3 += Matrix3x3
inline_ Matrix3x3& operator+=(const Matrix3x3& mat)
{
m[0][0] += mat.m[0][0]; m[0][1] += mat.m[0][1]; m[0][2] += mat.m[0][2];
m[1][0] += mat.m[1][0]; m[1][1] += mat.m[1][1]; m[1][2] += mat.m[1][2];
m[2][0] += mat.m[2][0]; m[2][1] += mat.m[2][1]; m[2][2] += mat.m[2][2];
return *this;
}
//! Operator for Matrix3x3 -= Matrix3x3
inline_ Matrix3x3& operator-=(const Matrix3x3& mat)
{
m[0][0] -= mat.m[0][0]; m[0][1] -= mat.m[0][1]; m[0][2] -= mat.m[0][2];
m[1][0] -= mat.m[1][0]; m[1][1] -= mat.m[1][1]; m[1][2] -= mat.m[1][2];
m[2][0] -= mat.m[2][0]; m[2][1] -= mat.m[2][1]; m[2][2] -= mat.m[2][2];
return *this;
}
//! Operator for Matrix3x3 *= Matrix3x3
inline_ Matrix3x3& operator*=(const Matrix3x3& mat)
{
Point TempRow;
GetRow(0, TempRow);
m[0][0] = TempRow.x*mat.m[0][0] + TempRow.y*mat.m[1][0] + TempRow.z*mat.m[2][0];
m[0][1] = TempRow.x*mat.m[0][1] + TempRow.y*mat.m[1][1] + TempRow.z*mat.m[2][1];
m[0][2] = TempRow.x*mat.m[0][2] + TempRow.y*mat.m[1][2] + TempRow.z*mat.m[2][2];
GetRow(1, TempRow);
m[1][0] = TempRow.x*mat.m[0][0] + TempRow.y*mat.m[1][0] + TempRow.z*mat.m[2][0];
m[1][1] = TempRow.x*mat.m[0][1] + TempRow.y*mat.m[1][1] + TempRow.z*mat.m[2][1];
m[1][2] = TempRow.x*mat.m[0][2] + TempRow.y*mat.m[1][2] + TempRow.z*mat.m[2][2];
GetRow(2, TempRow);
m[2][0] = TempRow.x*mat.m[0][0] + TempRow.y*mat.m[1][0] + TempRow.z*mat.m[2][0];
m[2][1] = TempRow.x*mat.m[0][1] + TempRow.y*mat.m[1][1] + TempRow.z*mat.m[2][1];
m[2][2] = TempRow.x*mat.m[0][2] + TempRow.y*mat.m[1][2] + TempRow.z*mat.m[2][2];
return *this;
}
//! Operator for Matrix3x3 *= float
inline_ Matrix3x3& operator*=(float s)
{
m[0][0] *= s; m[0][1] *= s; m[0][2] *= s;
m[1][0] *= s; m[1][1] *= s; m[1][2] *= s;
m[2][0] *= s; m[2][1] *= s; m[2][2] *= s;
return *this;
}
//! Operator for Matrix3x3 /= float
inline_ Matrix3x3& operator/=(float s)
{
if (s) s = 1.0f / s;
m[0][0] *= s; m[0][1] *= s; m[0][2] *= s;
m[1][0] *= s; m[1][1] *= s; m[1][2] *= s;
m[2][0] *= s; m[2][1] *= s; m[2][2] *= s;
return *this;
}
// Cast operators
//! Cast a Matrix3x3 to a Matrix4x4.
operator Matrix4x4() const;
//! Cast a Matrix3x3 to a Quat.
operator Quat() const;
inline_ const Point& operator[](int row) const { return *(const Point*)&m[row][0]; }
inline_ Point& operator[](int row) { return *(Point*)&m[row][0]; }
public:
float m[3][3];
};
#endif // __ICEMATRIX3X3_H__

135
ode/OPCODE/Ice/IceMatrix4x4.cpp Normal file
View File

@ -0,0 +1,135 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for 4x4 matrices.
* \file IceMatrix4x4.cpp
* \author Pierre Terdiman
* \date April, 4, 2000
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* 4x4 matrix.
* DirectX-compliant, ie row-column order, ie m[Row][Col].
* Same as:
* m11 m12 m13 m14 first row.
* m21 m22 m23 m24 second row.
* m31 m32 m33 m34 third row.
* m41 m42 m43 m44 fourth row.
* Translation is (m41, m42, m43), (m14, m24, m34, m44) = (0, 0, 0, 1).
* Stored in memory as m11 m12 m13 m14 m21...
*
* Multiplication rules:
*
* [x'y'z'1] = [xyz1][M]
*
* x' = x*m11 + y*m21 + z*m31 + m41
* y' = x*m12 + y*m22 + z*m32 + m42
* z' = x*m13 + y*m23 + z*m33 + m43
* 1' = 0 + 0 + 0 + m44
*
* \class Matrix4x4
* \author Pierre Terdiman
* \version 1.0
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Precompiled Header
#include "Stdafx.h"
using namespace IceMaths;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Inverts a PR matrix. (which only contains a rotation and a translation)
* This is faster and less subject to FPU errors than the generic inversion code.
*
* \relates Matrix4x4
* \fn InvertPRMatrix(Matrix4x4& dest, const Matrix4x4& src)
* \param dest [out] destination matrix
* \param src [in] source matrix
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ICEMATHS_API void IceMaths::InvertPRMatrix(Matrix4x4& dest, const Matrix4x4& src)
{
dest.m[0][0] = src.m[0][0];
dest.m[1][0] = src.m[0][1];
dest.m[2][0] = src.m[0][2];
dest.m[3][0] = -(src.m[3][0]*src.m[0][0] + src.m[3][1]*src.m[0][1] + src.m[3][2]*src.m[0][2]);
dest.m[0][1] = src.m[1][0];
dest.m[1][1] = src.m[1][1];
dest.m[2][1] = src.m[1][2];
dest.m[3][1] = -(src.m[3][0]*src.m[1][0] + src.m[3][1]*src.m[1][1] + src.m[3][2]*src.m[1][2]);
dest.m[0][2] = src.m[2][0];
dest.m[1][2] = src.m[2][1];
dest.m[2][2] = src.m[2][2];
dest.m[3][2] = -(src.m[3][0]*src.m[2][0] + src.m[3][1]*src.m[2][1] + src.m[3][2]*src.m[2][2]);
dest.m[0][3] = 0.0f;
dest.m[1][3] = 0.0f;
dest.m[2][3] = 0.0f;
dest.m[3][3] = 1.0f;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Compute the cofactor of the Matrix at a specified location
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
float Matrix4x4::CoFactor(udword row, udword col) const
{
return (( m[(row+1)&3][(col+1)&3]*m[(row+2)&3][(col+2)&3]*m[(row+3)&3][(col+3)&3] +
m[(row+1)&3][(col+2)&3]*m[(row+2)&3][(col+3)&3]*m[(row+3)&3][(col+1)&3] +
m[(row+1)&3][(col+3)&3]*m[(row+2)&3][(col+1)&3]*m[(row+3)&3][(col+2)&3])
- (m[(row+3)&3][(col+1)&3]*m[(row+2)&3][(col+2)&3]*m[(row+1)&3][(col+3)&3] +
m[(row+3)&3][(col+2)&3]*m[(row+2)&3][(col+3)&3]*m[(row+1)&3][(col+1)&3] +
m[(row+3)&3][(col+3)&3]*m[(row+2)&3][(col+1)&3]*m[(row+1)&3][(col+2)&3])) * ((row + col) & 1 ? -1.0f : +1.0f);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Compute the determinant of the Matrix
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
float Matrix4x4::Determinant() const
{
return m[0][0] * CoFactor(0, 0) +
m[0][1] * CoFactor(0, 1) +
m[0][2] * CoFactor(0, 2) +
m[0][3] * CoFactor(0, 3);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Compute the inverse of the matrix
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Matrix4x4& Matrix4x4::Invert()
{
float Det = Determinant();
Matrix4x4 Temp;
if(fabsf(Det) < MATRIX4X4_EPSILON)
return *this; // The matrix is not invertible! Singular case!
float IDet = 1.0f / Det;
Temp.m[0][0] = CoFactor(0,0) * IDet;
Temp.m[1][0] = CoFactor(0,1) * IDet;
Temp.m[2][0] = CoFactor(0,2) * IDet;
Temp.m[3][0] = CoFactor(0,3) * IDet;
Temp.m[0][1] = CoFactor(1,0) * IDet;
Temp.m[1][1] = CoFactor(1,1) * IDet;
Temp.m[2][1] = CoFactor(1,2) * IDet;
Temp.m[3][1] = CoFactor(1,3) * IDet;
Temp.m[0][2] = CoFactor(2,0) * IDet;
Temp.m[1][2] = CoFactor(2,1) * IDet;
Temp.m[2][2] = CoFactor(2,2) * IDet;
Temp.m[3][2] = CoFactor(2,3) * IDet;
Temp.m[0][3] = CoFactor(3,0) * IDet;
Temp.m[1][3] = CoFactor(3,1) * IDet;
Temp.m[2][3] = CoFactor(3,2) * IDet;
Temp.m[3][3] = CoFactor(3,3) * IDet;
*this = Temp;
return *this;
}

455
ode/OPCODE/Ice/IceMatrix4x4.h Normal file
View File

@ -0,0 +1,455 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for 4x4 matrices.
* \file IceMatrix4x4.h
* \author Pierre Terdiman
* \date April, 4, 2000
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __ICEMATRIX4X4_H__
#define __ICEMATRIX4X4_H__
// Forward declarations
class PRS;
class PR;
#define MATRIX4X4_EPSILON (1.0e-7f)
class ICEMATHS_API Matrix4x4
{
// void LUBackwardSubstitution( sdword *indx, float* b );
// void LUDecomposition( sdword* indx, float* d );
public:
//! Empty constructor.
inline_ Matrix4x4() {}
//! Constructor from 16 values
inline_ Matrix4x4( float m00, float m01, float m02, float m03,
float m10, float m11, float m12, float m13,
float m20, float m21, float m22, float m23,
float m30, float m31, float m32, float m33)
{
m[0][0] = m00; m[0][1] = m01; m[0][2] = m02; m[0][3] = m03;
m[1][0] = m10; m[1][1] = m11; m[1][2] = m12; m[1][3] = m13;
m[2][0] = m20; m[2][1] = m21; m[2][2] = m22; m[2][3] = m23;
m[3][0] = m30; m[3][1] = m31; m[3][2] = m32; m[3][3] = m33;
}
//! Copy constructor
inline_ Matrix4x4(const Matrix4x4& mat) { CopyMemory(m, &mat.m, 16*sizeof(float)); }
//! Destructor.
inline_ ~Matrix4x4() {}
//! Assign values (rotation only)
inline_ Matrix4x4& Set( float m00, float m01, float m02,
float m10, float m11, float m12,
float m20, float m21, float m22)
{
m[0][0] = m00; m[0][1] = m01; m[0][2] = m02;
m[1][0] = m10; m[1][1] = m11; m[1][2] = m12;
m[2][0] = m20; m[2][1] = m21; m[2][2] = m22;
return *this;
}
//! Assign values
inline_ Matrix4x4& Set( float m00, float m01, float m02, float m03,
float m10, float m11, float m12, float m13,
float m20, float m21, float m22, float m23,
float m30, float m31, float m32, float m33)
{
m[0][0] = m00; m[0][1] = m01; m[0][2] = m02; m[0][3] = m03;
m[1][0] = m10; m[1][1] = m11; m[1][2] = m12; m[1][3] = m13;
m[2][0] = m20; m[2][1] = m21; m[2][2] = m22; m[2][3] = m23;
m[3][0] = m30; m[3][1] = m31; m[3][2] = m32; m[3][3] = m33;
return *this;
}
//! Copy from a Matrix4x4
inline_ void Copy(const Matrix4x4& source) { CopyMemory(m, source.m, 16*sizeof(float)); }
// Row-column access
//! Returns a row.
inline_ void GetRow(const udword r, HPoint& p) const { p.x=m[r][0]; p.y=m[r][1]; p.z=m[r][2]; p.w=m[r][3]; }
//! Returns a row.
inline_ void GetRow(const udword r, Point& p) const { p.x=m[r][0]; p.y=m[r][1]; p.z=m[r][2]; }
//! Returns a row.
inline_ const HPoint& GetRow(const udword r) const { return *(const HPoint*)&m[r][0]; }
//! Returns a row.
inline_ HPoint& GetRow(const udword r) { return *(HPoint*)&m[r][0]; }
//! Sets a row.
inline_ void SetRow(const udword r, const HPoint& p) { m[r][0]=p.x; m[r][1]=p.y; m[r][2]=p.z; m[r][3]=p.w; }
//! Sets a row.
inline_ void SetRow(const udword r, const Point& p) { m[r][0]=p.x; m[r][1]=p.y; m[r][2]=p.z; m[r][3]= (r!=3) ? 0.0f : 1.0f; }
//! Returns a column.
inline_ void GetCol(const udword c, HPoint& p) const { p.x=m[0][c]; p.y=m[1][c]; p.z=m[2][c]; p.w=m[3][c]; }
//! Returns a column.
inline_ void GetCol(const udword c, Point& p) const { p.x=m[0][c]; p.y=m[1][c]; p.z=m[2][c]; }
//! Sets a column.
inline_ void SetCol(const udword c, const HPoint& p) { m[0][c]=p.x; m[1][c]=p.y; m[2][c]=p.z; m[3][c]=p.w; }
//! Sets a column.
inline_ void SetCol(const udword c, const Point& p) { m[0][c]=p.x; m[1][c]=p.y; m[2][c]=p.z; m[3][c]= (c!=3) ? 0.0f : 1.0f; }
// Translation
//! Returns the translation part of the matrix.
inline_ const HPoint& GetTrans() const { return GetRow(3); }
//! Gets the translation part of the matrix
inline_ void GetTrans(Point& p) const { p.x=m[3][0]; p.y=m[3][1]; p.z=m[3][2]; }
//! Sets the translation part of the matrix, from a Point.
inline_ void SetTrans(const Point& p) { m[3][0]=p.x; m[3][1]=p.y; m[3][2]=p.z; }
//! Sets the translation part of the matrix, from a HPoint.
inline_ void SetTrans(const HPoint& p) { m[3][0]=p.x; m[3][1]=p.y; m[3][2]=p.z; m[3][3]=p.w; }
//! Sets the translation part of the matrix, from floats.
inline_ void SetTrans(float tx, float ty, float tz) { m[3][0]=tx; m[3][1]=ty; m[3][2]=tz; }
// Scale
//! Sets the scale from a Point. The point is put on the diagonal.
inline_ void SetScale(const Point& p) { m[0][0]=p.x; m[1][1]=p.y; m[2][2]=p.z; }
//! Sets the scale from floats. Values are put on the diagonal.
inline_ void SetScale(float sx, float sy, float sz) { m[0][0]=sx; m[1][1]=sy; m[2][2]=sz; }
//! Scales from a Point. Each row is multiplied by a component.
void Scale(const Point& p)
{
m[0][0] *= p.x; m[1][0] *= p.y; m[2][0] *= p.z;
m[0][1] *= p.x; m[1][1] *= p.y; m[2][1] *= p.z;
m[0][2] *= p.x; m[1][2] *= p.y; m[2][2] *= p.z;
}
//! Scales from floats. Each row is multiplied by a value.
void Scale(float sx, float sy, float sz)
{
m[0][0] *= sx; m[1][0] *= sy; m[2][0] *= sz;
m[0][1] *= sx; m[1][1] *= sy; m[2][1] *= sz;
m[0][2] *= sx; m[1][2] *= sy; m[2][2] *= sz;
}
/*
//! Returns a row.
inline_ HPoint GetRow(const udword row) const { return mRow[row]; }
//! Sets a row.
inline_ Matrix4x4& SetRow(const udword row, const HPoint& p) { mRow[row] = p; return *this; }
//! Sets a row.
Matrix4x4& SetRow(const udword row, const Point& p)
{
m[row][0] = p.x;
m[row][1] = p.y;
m[row][2] = p.z;
m[row][3] = (row != 3) ? 0.0f : 1.0f;
return *this;
}
//! Returns a column.
HPoint GetCol(const udword col) const
{
HPoint Res;
Res.x = m[0][col];
Res.y = m[1][col];
Res.z = m[2][col];
Res.w = m[3][col];
return Res;
}
//! Sets a column.
Matrix4x4& SetCol(const udword col, const HPoint& p)
{
m[0][col] = p.x;
m[1][col] = p.y;
m[2][col] = p.z;
m[3][col] = p.w;
return *this;
}
//! Sets a column.
Matrix4x4& SetCol(const udword col, const Point& p)
{
m[0][col] = p.x;
m[1][col] = p.y;
m[2][col] = p.z;
m[3][col] = (col != 3) ? 0.0f : 1.0f;
return *this;
}
*/
//! Computes the trace. The trace is the sum of the 4 diagonal components.
inline_ float Trace() const { return m[0][0] + m[1][1] + m[2][2] + m[3][3]; }
//! Computes the trace of the upper 3x3 matrix.
inline_ float Trace3x3() const { return m[0][0] + m[1][1] + m[2][2]; }
//! Clears the matrix.
inline_ void Zero() { ZeroMemory(&m, sizeof(m)); }
//! Sets the identity matrix.
inline_ void Identity() { Zero(); m[0][0] = m[1][1] = m[2][2] = m[3][3] = 1.0f; }
//! Checks for identity
inline_ bool IsIdentity() const
{
if(IR(m[0][0])!=IEEE_1_0) return false;
if(IR(m[0][1])!=0) return false;
if(IR(m[0][2])!=0) return false;
if(IR(m[0][3])!=0) return false;
if(IR(m[1][0])!=0) return false;
if(IR(m[1][1])!=IEEE_1_0) return false;
if(IR(m[1][2])!=0) return false;
if(IR(m[1][3])!=0) return false;
if(IR(m[2][0])!=0) return false;
if(IR(m[2][1])!=0) return false;
if(IR(m[2][2])!=IEEE_1_0) return false;
if(IR(m[2][3])!=0) return false;
if(IR(m[3][0])!=0) return false;
if(IR(m[3][1])!=0) return false;
if(IR(m[3][2])!=0) return false;
if(IR(m[3][3])!=IEEE_1_0) return false;
return true;
}
//! Checks matrix validity
inline_ BOOL IsValid() const
{
for(udword j=0;j<4;j++)
{
for(udword i=0;i<4;i++)
{
if(!IsValidFloat(m[j][i])) return FALSE;
}
}
return TRUE;
}
//! Sets a rotation matrix around the X axis.
void RotX(float angle) { float Cos = cosf(angle), Sin = sinf(angle); Identity(); m[1][1] = m[2][2] = Cos; m[2][1] = -Sin; m[1][2] = Sin; }
//! Sets a rotation matrix around the Y axis.
void RotY(float angle) { float Cos = cosf(angle), Sin = sinf(angle); Identity(); m[0][0] = m[2][2] = Cos; m[2][0] = Sin; m[0][2] = -Sin; }
//! Sets a rotation matrix around the Z axis.
void RotZ(float angle) { float Cos = cosf(angle), Sin = sinf(angle); Identity(); m[0][0] = m[1][1] = Cos; m[1][0] = -Sin; m[0][1] = Sin; }
//! Makes a rotation matrix about an arbitrary axis
Matrix4x4& Rot(float angle, Point& p1, Point& p2);
//! Transposes the matrix.
void Transpose()
{
IR(m[1][0]) ^= IR(m[0][1]); IR(m[0][1]) ^= IR(m[1][0]); IR(m[1][0]) ^= IR(m[0][1]);
IR(m[2][0]) ^= IR(m[0][2]); IR(m[0][2]) ^= IR(m[2][0]); IR(m[2][0]) ^= IR(m[0][2]);
IR(m[3][0]) ^= IR(m[0][3]); IR(m[0][3]) ^= IR(m[3][0]); IR(m[3][0]) ^= IR(m[0][3]);
IR(m[1][2]) ^= IR(m[2][1]); IR(m[2][1]) ^= IR(m[1][2]); IR(m[1][2]) ^= IR(m[2][1]);
IR(m[1][3]) ^= IR(m[3][1]); IR(m[3][1]) ^= IR(m[1][3]); IR(m[1][3]) ^= IR(m[3][1]);
IR(m[2][3]) ^= IR(m[3][2]); IR(m[3][2]) ^= IR(m[2][3]); IR(m[2][3]) ^= IR(m[3][2]);
}
//! Computes a cofactor. Used for matrix inversion.
float CoFactor(udword row, udword col) const;
//! Computes the determinant of the matrix.
float Determinant() const;
//! Inverts the matrix. Determinant must be different from zero, else matrix can't be inverted.
Matrix4x4& Invert();
// Matrix& ComputeAxisMatrix(Point& axis, float angle);
// Cast operators
//! Casts a Matrix4x4 to a Matrix3x3.
inline_ operator Matrix3x3() const
{
return Matrix3x3(
m[0][0], m[0][1], m[0][2],
m[1][0], m[1][1], m[1][2],
m[2][0], m[2][1], m[2][2]);
}
//! Casts a Matrix4x4 to a Quat.
operator Quat() const;
//! Casts a Matrix4x4 to a PR.
operator PR() const;
// Arithmetic operators
//! Operator for Matrix4x4 Plus = Matrix4x4 + Matrix4x4;
inline_ Matrix4x4 operator+(const Matrix4x4& mat) const
{
return Matrix4x4(
m[0][0]+mat.m[0][0], m[0][1]+mat.m[0][1], m[0][2]+mat.m[0][2], m[0][3]+mat.m[0][3],
m[1][0]+mat.m[1][0], m[1][1]+mat.m[1][1], m[1][2]+mat.m[1][2], m[1][3]+mat.m[1][3],
m[2][0]+mat.m[2][0], m[2][1]+mat.m[2][1], m[2][2]+mat.m[2][2], m[2][3]+mat.m[2][3],
m[3][0]+mat.m[3][0], m[3][1]+mat.m[3][1], m[3][2]+mat.m[3][2], m[3][3]+mat.m[3][3]);
}
//! Operator for Matrix4x4 Minus = Matrix4x4 - Matrix4x4;
inline_ Matrix4x4 operator-(const Matrix4x4& mat) const
{
return Matrix4x4(
m[0][0]-mat.m[0][0], m[0][1]-mat.m[0][1], m[0][2]-mat.m[0][2], m[0][3]-mat.m[0][3],
m[1][0]-mat.m[1][0], m[1][1]-mat.m[1][1], m[1][2]-mat.m[1][2], m[1][3]-mat.m[1][3],
m[2][0]-mat.m[2][0], m[2][1]-mat.m[2][1], m[2][2]-mat.m[2][2], m[2][3]-mat.m[2][3],
m[3][0]-mat.m[3][0], m[3][1]-mat.m[3][1], m[3][2]-mat.m[3][2], m[3][3]-mat.m[3][3]);
}
//! Operator for Matrix4x4 Mul = Matrix4x4 * Matrix4x4;
inline_ Matrix4x4 operator*(const Matrix4x4& mat) const
{
return Matrix4x4(
m[0][0]*mat.m[0][0] + m[0][1]*mat.m[1][0] + m[0][2]*mat.m[2][0] + m[0][3]*mat.m[3][0],
m[0][0]*mat.m[0][1] + m[0][1]*mat.m[1][1] + m[0][2]*mat.m[2][1] + m[0][3]*mat.m[3][1],
m[0][0]*mat.m[0][2] + m[0][1]*mat.m[1][2] + m[0][2]*mat.m[2][2] + m[0][3]*mat.m[3][2],
m[0][0]*mat.m[0][3] + m[0][1]*mat.m[1][3] + m[0][2]*mat.m[2][3] + m[0][3]*mat.m[3][3],
m[1][0]*mat.m[0][0] + m[1][1]*mat.m[1][0] + m[1][2]*mat.m[2][0] + m[1][3]*mat.m[3][0],
m[1][0]*mat.m[0][1] + m[1][1]*mat.m[1][1] + m[1][2]*mat.m[2][1] + m[1][3]*mat.m[3][1],
m[1][0]*mat.m[0][2] + m[1][1]*mat.m[1][2] + m[1][2]*mat.m[2][2] + m[1][3]*mat.m[3][2],
m[1][0]*mat.m[0][3] + m[1][1]*mat.m[1][3] + m[1][2]*mat.m[2][3] + m[1][3]*mat.m[3][3],
m[2][0]*mat.m[0][0] + m[2][1]*mat.m[1][0] + m[2][2]*mat.m[2][0] + m[2][3]*mat.m[3][0],
m[2][0]*mat.m[0][1] + m[2][1]*mat.m[1][1] + m[2][2]*mat.m[2][1] + m[2][3]*mat.m[3][1],
m[2][0]*mat.m[0][2] + m[2][1]*mat.m[1][2] + m[2][2]*mat.m[2][2] + m[2][3]*mat.m[3][2],
m[2][0]*mat.m[0][3] + m[2][1]*mat.m[1][3] + m[2][2]*mat.m[2][3] + m[2][3]*mat.m[3][3],
m[3][0]*mat.m[0][0] + m[3][1]*mat.m[1][0] + m[3][2]*mat.m[2][0] + m[3][3]*mat.m[3][0],
m[3][0]*mat.m[0][1] + m[3][1]*mat.m[1][1] + m[3][2]*mat.m[2][1] + m[3][3]*mat.m[3][1],
m[3][0]*mat.m[0][2] + m[3][1]*mat.m[1][2] + m[3][2]*mat.m[2][2] + m[3][3]*mat.m[3][2],
m[3][0]*mat.m[0][3] + m[3][1]*mat.m[1][3] + m[3][2]*mat.m[2][3] + m[3][3]*mat.m[3][3]);
}
//! Operator for HPoint Mul = Matrix4x4 * HPoint;
inline_ HPoint operator*(const HPoint& v) const { return HPoint(GetRow(0)|v, GetRow(1)|v, GetRow(2)|v, GetRow(3)|v); }
//! Operator for Point Mul = Matrix4x4 * Point;
inline_ Point operator*(const Point& v) const
{
return Point( m[0][0]*v.x + m[0][1]*v.y + m[0][2]*v.z + m[0][3],
m[1][0]*v.x + m[1][1]*v.y + m[1][2]*v.z + m[1][3],
m[2][0]*v.x + m[2][1]*v.y + m[2][2]*v.z + m[2][3] );
}
//! Operator for Matrix4x4 Scale = Matrix4x4 * float;
inline_ Matrix4x4 operator*(float s) const
{
return Matrix4x4(
m[0][0]*s, m[0][1]*s, m[0][2]*s, m[0][3]*s,
m[1][0]*s, m[1][1]*s, m[1][2]*s, m[1][3]*s,
m[2][0]*s, m[2][1]*s, m[2][2]*s, m[2][3]*s,
m[3][0]*s, m[3][1]*s, m[3][2]*s, m[3][3]*s);
}
//! Operator for Matrix4x4 Scale = float * Matrix4x4;
inline_ friend Matrix4x4 operator*(float s, const Matrix4x4& mat)
{
return Matrix4x4(
s*mat.m[0][0], s*mat.m[0][1], s*mat.m[0][2], s*mat.m[0][3],
s*mat.m[1][0], s*mat.m[1][1], s*mat.m[1][2], s*mat.m[1][3],
s*mat.m[2][0], s*mat.m[2][1], s*mat.m[2][2], s*mat.m[2][3],
s*mat.m[3][0], s*mat.m[3][1], s*mat.m[3][2], s*mat.m[3][3]);
}
//! Operator for Matrix4x4 Div = Matrix4x4 / float;
inline_ Matrix4x4 operator/(float s) const
{
if(s) s = 1.0f / s;
return Matrix4x4(
m[0][0]*s, m[0][1]*s, m[0][2]*s, m[0][3]*s,
m[1][0]*s, m[1][1]*s, m[1][2]*s, m[1][3]*s,
m[2][0]*s, m[2][1]*s, m[2][2]*s, m[2][3]*s,
m[3][0]*s, m[3][1]*s, m[3][2]*s, m[3][3]*s);
}
//! Operator for Matrix4x4 Div = float / Matrix4x4;
inline_ friend Matrix4x4 operator/(float s, const Matrix4x4& mat)
{
return Matrix4x4(
s/mat.m[0][0], s/mat.m[0][1], s/mat.m[0][2], s/mat.m[0][3],
s/mat.m[1][0], s/mat.m[1][1], s/mat.m[1][2], s/mat.m[1][3],
s/mat.m[2][0], s/mat.m[2][1], s/mat.m[2][2], s/mat.m[2][3],
s/mat.m[3][0], s/mat.m[3][1], s/mat.m[3][2], s/mat.m[3][3]);
}
//! Operator for Matrix4x4 += Matrix4x4;
inline_ Matrix4x4& operator+=(const Matrix4x4& mat)
{
m[0][0]+=mat.m[0][0]; m[0][1]+=mat.m[0][1]; m[0][2]+=mat.m[0][2]; m[0][3]+=mat.m[0][3];
m[1][0]+=mat.m[1][0]; m[1][1]+=mat.m[1][1]; m[1][2]+=mat.m[1][2]; m[1][3]+=mat.m[1][3];
m[2][0]+=mat.m[2][0]; m[2][1]+=mat.m[2][1]; m[2][2]+=mat.m[2][2]; m[2][3]+=mat.m[2][3];
m[3][0]+=mat.m[3][0]; m[3][1]+=mat.m[3][1]; m[3][2]+=mat.m[3][2]; m[3][3]+=mat.m[3][3];
return *this;
}
//! Operator for Matrix4x4 -= Matrix4x4;
inline_ Matrix4x4& operator-=(const Matrix4x4& mat)
{
m[0][0]-=mat.m[0][0]; m[0][1]-=mat.m[0][1]; m[0][2]-=mat.m[0][2]; m[0][3]-=mat.m[0][3];
m[1][0]-=mat.m[1][0]; m[1][1]-=mat.m[1][1]; m[1][2]-=mat.m[1][2]; m[1][3]-=mat.m[1][3];
m[2][0]-=mat.m[2][0]; m[2][1]-=mat.m[2][1]; m[2][2]-=mat.m[2][2]; m[2][3]-=mat.m[2][3];
m[3][0]-=mat.m[3][0]; m[3][1]-=mat.m[3][1]; m[3][2]-=mat.m[3][2]; m[3][3]-=mat.m[3][3];
return *this;
}
//! Operator for Matrix4x4 *= Matrix4x4;
Matrix4x4& operator*=(const Matrix4x4& mat)
{
HPoint TempRow;
GetRow(0, TempRow);
m[0][0] = TempRow.x*mat.m[0][0] + TempRow.y*mat.m[1][0] + TempRow.z*mat.m[2][0] + TempRow.w*mat.m[3][0];
m[0][1] = TempRow.x*mat.m[0][1] + TempRow.y*mat.m[1][1] + TempRow.z*mat.m[2][1] + TempRow.w*mat.m[3][1];
m[0][2] = TempRow.x*mat.m[0][2] + TempRow.y*mat.m[1][2] + TempRow.z*mat.m[2][2] + TempRow.w*mat.m[3][2];
m[0][3] = TempRow.x*mat.m[0][3] + TempRow.y*mat.m[1][3] + TempRow.z*mat.m[2][3] + TempRow.w*mat.m[3][3];
GetRow(1, TempRow);
m[1][0] = TempRow.x*mat.m[0][0] + TempRow.y*mat.m[1][0] + TempRow.z*mat.m[2][0] + TempRow.w*mat.m[3][0];
m[1][1] = TempRow.x*mat.m[0][1] + TempRow.y*mat.m[1][1] + TempRow.z*mat.m[2][1] + TempRow.w*mat.m[3][1];
m[1][2] = TempRow.x*mat.m[0][2] + TempRow.y*mat.m[1][2] + TempRow.z*mat.m[2][2] + TempRow.w*mat.m[3][2];
m[1][3] = TempRow.x*mat.m[0][3] + TempRow.y*mat.m[1][3] + TempRow.z*mat.m[2][3] + TempRow.w*mat.m[3][3];
GetRow(2, TempRow);
m[2][0] = TempRow.x*mat.m[0][0] + TempRow.y*mat.m[1][0] + TempRow.z*mat.m[2][0] + TempRow.w*mat.m[3][0];
m[2][1] = TempRow.x*mat.m[0][1] + TempRow.y*mat.m[1][1] + TempRow.z*mat.m[2][1] + TempRow.w*mat.m[3][1];
m[2][2] = TempRow.x*mat.m[0][2] + TempRow.y*mat.m[1][2] + TempRow.z*mat.m[2][2] + TempRow.w*mat.m[3][2];
m[2][3] = TempRow.x*mat.m[0][3] + TempRow.y*mat.m[1][3] + TempRow.z*mat.m[2][3] + TempRow.w*mat.m[3][3];
GetRow(3, TempRow);
m[3][0] = TempRow.x*mat.m[0][0] + TempRow.y*mat.m[1][0] + TempRow.z*mat.m[2][0] + TempRow.w*mat.m[3][0];
m[3][1] = TempRow.x*mat.m[0][1] + TempRow.y*mat.m[1][1] + TempRow.z*mat.m[2][1] + TempRow.w*mat.m[3][1];
m[3][2] = TempRow.x*mat.m[0][2] + TempRow.y*mat.m[1][2] + TempRow.z*mat.m[2][2] + TempRow.w*mat.m[3][2];
m[3][3] = TempRow.x*mat.m[0][3] + TempRow.y*mat.m[1][3] + TempRow.z*mat.m[2][3] + TempRow.w*mat.m[3][3];
return *this;
}
//! Operator for Matrix4x4 *= float;
inline_ Matrix4x4& operator*=(float s)
{
m[0][0]*=s; m[0][1]*=s; m[0][2]*=s; m[0][3]*=s;
m[1][0]*=s; m[1][1]*=s; m[1][2]*=s; m[1][3]*=s;
m[2][0]*=s; m[2][1]*=s; m[2][2]*=s; m[2][3]*=s;
m[3][0]*=s; m[3][1]*=s; m[3][2]*=s; m[3][3]*=s;
return *this;
}
//! Operator for Matrix4x4 /= float;
inline_ Matrix4x4& operator/=(float s)
{
if(s) s = 1.0f / s;
m[0][0]*=s; m[0][1]*=s; m[0][2]*=s; m[0][3]*=s;
m[1][0]*=s; m[1][1]*=s; m[1][2]*=s; m[1][3]*=s;
m[2][0]*=s; m[2][1]*=s; m[2][2]*=s; m[2][3]*=s;
m[3][0]*=s; m[3][1]*=s; m[3][2]*=s; m[3][3]*=s;
return *this;
}
inline_ const HPoint& operator[](int row) const { return *(const HPoint*)&m[row][0]; }
inline_ HPoint& operator[](int row) { return *(HPoint*)&m[row][0]; }
public:
float m[4][4];
};
//! Quickly rotates & translates a vector, using the 4x3 part of a 4x4 matrix
inline_ void TransformPoint4x3(Point& dest, const Point& source, const Matrix4x4& rot)
{
dest.x = rot.m[3][0] + source.x * rot.m[0][0] + source.y * rot.m[1][0] + source.z * rot.m[2][0];
dest.y = rot.m[3][1] + source.x * rot.m[0][1] + source.y * rot.m[1][1] + source.z * rot.m[2][1];
dest.z = rot.m[3][2] + source.x * rot.m[0][2] + source.y * rot.m[1][2] + source.z * rot.m[2][2];
}
//! Quickly rotates a vector, using the 3x3 part of a 4x4 matrix
inline_ void TransformPoint3x3(Point& dest, const Point& source, const Matrix4x4& rot)
{
dest.x = source.x * rot.m[0][0] + source.y * rot.m[1][0] + source.z * rot.m[2][0];
dest.y = source.x * rot.m[0][1] + source.y * rot.m[1][1] + source.z * rot.m[2][1];
dest.z = source.x * rot.m[0][2] + source.y * rot.m[1][2] + source.z * rot.m[2][2];
}
ICEMATHS_API void InvertPRMatrix(Matrix4x4& dest, const Matrix4x4& src);
#endif // __ICEMATRIX4X4_H__

109
ode/OPCODE/Ice/IceMemoryMacros.h Normal file
View File

@ -0,0 +1,109 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains all memory macros.
* \file IceMemoryMacros.h
* \author Pierre Terdiman
* \date April, 4, 2000
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __ICEMEMORYMACROS_H__
#define __ICEMEMORYMACROS_H__
#undef ZeroMemory
#undef CopyMemory
#undef MoveMemory
#undef FillMemory
//! Clears a buffer.
//! \param addr [in] buffer address
//! \param size [in] buffer length
//! \see FillMemory
//! \see StoreDwords
//! \see CopyMemory
//! \see MoveMemory
inline_ void ZeroMemory(void* addr, udword size) { memset(addr, 0, size); }
//! Fills a buffer with a given byte.
//! \param addr [in] buffer address
//! \param size [in] buffer length
//! \param val [in] the byte value
//! \see StoreDwords
//! \see ZeroMemory
//! \see CopyMemory
//! \see MoveMemory
inline_ void FillMemory(void* dest, udword size, ubyte val) { memset(dest, val, size); }
//! Fills a buffer with a given dword.
//! \param addr [in] buffer address
//! \param nb [in] number of dwords to write
//! \param value [in] the dword value
//! \see FillMemory
//! \see ZeroMemory
//! \see CopyMemory
//! \see MoveMemory
//! \warning writes nb*4 bytes !
inline_ void StoreDwords(udword* dest, udword nb, udword value)
{
// The asm code below **SHOULD** be equivalent to one of those C versions
// or the other if your compiled is good: (checked on VC++ 6.0)
//
// 1) while(nb--) *dest++ = value;
//
// 2) for(udword i=0;i<nb;i++) dest[i] = value;
//
#ifdef _MSC_VER
_asm push eax
_asm push ecx
_asm push edi
_asm mov edi, dest
_asm mov ecx, nb
_asm mov eax, value
_asm rep stosd
_asm pop edi
_asm pop ecx
_asm pop eax
#else
while(nb--) *dest++ = value;
#endif
}
//! Copies a buffer.
//! \param addr [in] destination buffer address
//! \param addr [in] source buffer address
//! \param size [in] buffer length
//! \see ZeroMemory
//! \see FillMemory
//! \see StoreDwords
//! \see MoveMemory
inline_ void CopyMemory(void* dest, const void* src, udword size) { memcpy(dest, src, size); }
//! Moves a buffer.
//! \param addr [in] destination buffer address
//! \param addr [in] source buffer address
//! \param size [in] buffer length
//! \see ZeroMemory
//! \see FillMemory
//! \see StoreDwords
//! \see CopyMemory
inline_ void MoveMemory(void* dest, const void* src, udword size) { memmove(dest, src, size); }
#define SIZEOFOBJECT sizeof(*this) //!< Gives the size of current object. Avoid some mistakes (e.g. "sizeof(this)").
//#define CLEAROBJECT { memset(this, 0, SIZEOFOBJECT); } //!< Clears current object. Laziness is my business. HANDLE WITH CARE.
#define DELETESINGLE(x) if (x) { delete x; x = null; } //!< Deletes an instance of a class.
#define DELETEARRAY(x) if (x) { delete []x; x = null; } //!< Deletes an array.
#define SAFE_RELEASE(x) if (x) { (x)->Release(); (x) = null; } //!< Safe D3D-style release
#define SAFE_DESTRUCT(x) if (x) { (x)->SelfDestruct(); (x) = null; } //!< Safe ICE-style release
#ifdef __ICEERROR_H__
#define CHECKALLOC(x) if(!x) return SetIceError("Out of memory.", EC_OUT_OF_MEMORY); //!< Standard alloc checking. HANDLE WITH CARE.
#else
#define CHECKALLOC(x) if(!x) return false;
#endif
//! Standard allocation cycle
#define SAFE_ALLOC(ptr, type, count) DELETEARRAY(ptr); ptr = new type[count]; CHECKALLOC(ptr);
#endif // __ICEMEMORYMACROS_H__

323
ode/OPCODE/Ice/IceOBB.cpp Normal file
View File

@ -0,0 +1,323 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains OBB-related code.
* \file IceOBB.cpp
* \author Pierre Terdiman
* \date January, 29, 2000
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* An Oriented Bounding Box (OBB).
* \class OBB
* \author Pierre Terdiman
* \version 1.0
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Precompiled Header
#include "Stdafx.h"
using namespace IceMaths;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Tests if a point is contained within the OBB.
* \param p [in] the world point to test
* \return true if inside the OBB
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool OBB::ContainsPoint(const Point& p) const
{
// Point in OBB test using lazy evaluation and early exits
// Translate to box space
Point RelPoint = p - mCenter;
// Point * mRot maps from box space to world space
// mRot * Point maps from world space to box space (what we need here)
float f = mRot.m[0][0] * RelPoint.x + mRot.m[0][1] * RelPoint.y + mRot.m[0][2] * RelPoint.z;
if(f >= mExtents.x || f <= -mExtents.x) return false;
f = mRot.m[1][0] * RelPoint.x + mRot.m[1][1] * RelPoint.y + mRot.m[1][2] * RelPoint.z;
if(f >= mExtents.y || f <= -mExtents.y) return false;
f = mRot.m[2][0] * RelPoint.x + mRot.m[2][1] * RelPoint.y + mRot.m[2][2] * RelPoint.z;
if(f >= mExtents.z || f <= -mExtents.z) return false;
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Builds an OBB from an AABB and a world transform.
* \param aabb [in] the aabb
* \param mat [in] the world transform
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void OBB::Create(const AABB& aabb, const Matrix4x4& mat)
{
// Note: must be coherent with Rotate()
aabb.GetCenter(mCenter);
aabb.GetExtents(mExtents);
// Here we have the same as OBB::Rotate(mat) where the obb is (mCenter, mExtents, Identity).
// So following what's done in Rotate:
// - x-form the center
mCenter *= mat;
// - combine rotation with identity, i.e. just use given matrix
mRot = mat;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the obb planes.
* \param planes [out] 6 box planes
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool OBB::ComputePlanes(Plane* planes) const
{
// Checkings
if(!planes) return false;
Point Axis0 = mRot[0];
Point Axis1 = mRot[1];
Point Axis2 = mRot[2];
// Writes normals
planes[0].n = Axis0;
planes[1].n = -Axis0;
planes[2].n = Axis1;
planes[3].n = -Axis1;
planes[4].n = Axis2;
planes[5].n = -Axis2;
// Compute a point on each plane
Point p0 = mCenter + Axis0 * mExtents.x;
Point p1 = mCenter - Axis0 * mExtents.x;
Point p2 = mCenter + Axis1 * mExtents.y;
Point p3 = mCenter - Axis1 * mExtents.y;
Point p4 = mCenter + Axis2 * mExtents.z;
Point p5 = mCenter - Axis2 * mExtents.z;
// Compute d
planes[0].d = -(planes[0].n|p0);
planes[1].d = -(planes[1].n|p1);
planes[2].d = -(planes[2].n|p2);
planes[3].d = -(planes[3].n|p3);
planes[4].d = -(planes[4].n|p4);
planes[5].d = -(planes[5].n|p5);
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the obb points.
* \param pts [out] 8 box points
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool OBB::ComputePoints(Point* pts) const
{
// Checkings
if(!pts) return false;
Point Axis0 = mRot[0];
Point Axis1 = mRot[1];
Point Axis2 = mRot[2];
Axis0 *= mExtents.x;
Axis1 *= mExtents.y;
Axis2 *= mExtents.z;
// 7+------+6 0 = ---
// /| /| 1 = +--
// / | / | 2 = ++-
// / 4+---/--+5 3 = -+-
// 3+------+2 / y z 4 = --+
// | / | / | / 5 = +-+
// |/ |/ |/ 6 = +++
// 0+------+1 *---x 7 = -++
pts[0] = mCenter - Axis0 - Axis1 - Axis2;
pts[1] = mCenter + Axis0 - Axis1 - Axis2;
pts[2] = mCenter + Axis0 + Axis1 - Axis2;
pts[3] = mCenter - Axis0 + Axis1 - Axis2;
pts[4] = mCenter - Axis0 - Axis1 + Axis2;
pts[5] = mCenter + Axis0 - Axis1 + Axis2;
pts[6] = mCenter + Axis0 + Axis1 + Axis2;
pts[7] = mCenter - Axis0 + Axis1 + Axis2;
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes vertex normals.
* \param pts [out] 8 box points
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool OBB::ComputeVertexNormals(Point* pts) const
{
static float VertexNormals[] =
{
-INVSQRT3, -INVSQRT3, -INVSQRT3,
INVSQRT3, -INVSQRT3, -INVSQRT3,
INVSQRT3, INVSQRT3, -INVSQRT3,
-INVSQRT3, INVSQRT3, -INVSQRT3,
-INVSQRT3, -INVSQRT3, INVSQRT3,
INVSQRT3, -INVSQRT3, INVSQRT3,
INVSQRT3, INVSQRT3, INVSQRT3,
-INVSQRT3, INVSQRT3, INVSQRT3
};
if(!pts) return false;
const Point* VN = (const Point*)VertexNormals;
for(udword i=0;i<8;i++)
{
pts[i] = VN[i] * mRot;
}
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Returns edges.
* \return 24 indices (12 edges) indexing the list returned by ComputePoints()
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
const udword* OBB::GetEdges() const
{
static udword Indices[] = {
0, 1, 1, 2, 2, 3, 3, 0,
7, 6, 6, 5, 5, 4, 4, 7,
1, 5, 6, 2,
3, 7, 4, 0
};
return Indices;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Returns local edge normals.
* \return edge normals in local space
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
const Point* OBB::GetLocalEdgeNormals() const
{
static float EdgeNormals[] =
{
0, -INVSQRT2, -INVSQRT2, // 0-1
INVSQRT2, 0, -INVSQRT2, // 1-2
0, INVSQRT2, -INVSQRT2, // 2-3
-INVSQRT2, 0, -INVSQRT2, // 3-0
0, INVSQRT2, INVSQRT2, // 7-6
INVSQRT2, 0, INVSQRT2, // 6-5
0, -INVSQRT2, INVSQRT2, // 5-4
-INVSQRT2, 0, INVSQRT2, // 4-7
INVSQRT2, -INVSQRT2, 0, // 1-5
INVSQRT2, INVSQRT2, 0, // 6-2
-INVSQRT2, INVSQRT2, 0, // 3-7
-INVSQRT2, -INVSQRT2, 0 // 4-0
};
return (const Point*)EdgeNormals;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Returns world edge normal
* \param edge_index [in] 0 <= edge index < 12
* \param world_normal [out] edge normal in world space
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void OBB::ComputeWorldEdgeNormal(udword edge_index, Point& world_normal) const
{
ASSERT(edge_index<12);
world_normal = GetLocalEdgeNormals()[edge_index] * mRot;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes an LSS surrounding the OBB.
* \param lss [out] the LSS
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void OBB::ComputeLSS(LSS& lss) const
{
Point Axis0 = mRot[0];
Point Axis1 = mRot[1];
Point Axis2 = mRot[2];
switch(mExtents.LargestAxis())
{
case 0:
lss.mRadius = (mExtents.y + mExtents.z)*0.5f;
lss.mP0 = mCenter + Axis0 * (mExtents.x - lss.mRadius);
lss.mP1 = mCenter - Axis0 * (mExtents.x - lss.mRadius);
break;
case 1:
lss.mRadius = (mExtents.x + mExtents.z)*0.5f;
lss.mP0 = mCenter + Axis1 * (mExtents.y - lss.mRadius);
lss.mP1 = mCenter - Axis1 * (mExtents.y - lss.mRadius);
break;
case 2:
lss.mRadius = (mExtents.x + mExtents.y)*0.5f;
lss.mP0 = mCenter + Axis2 * (mExtents.z - lss.mRadius);
lss.mP1 = mCenter - Axis2 * (mExtents.z - lss.mRadius);
break;
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Checks the OBB is inside another OBB.
* \param box [in] the other OBB
* \return TRUE if we're inside the other box
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
BOOL OBB::IsInside(const OBB& box) const
{
// Make a 4x4 from the box & inverse it
Matrix4x4 M0Inv;
{
Matrix4x4 M0 = box.mRot;
M0.SetTrans(box.mCenter);
InvertPRMatrix(M0Inv, M0);
}
// With our inversed 4x4, create box1 in space of box0
OBB _1in0;
Rotate(M0Inv, _1in0);
// This should cancel out box0's rotation, i.e. it's now an AABB.
// => Center(0,0,0), Rot(identity)
// The two boxes are in the same space so now we can compare them.
// Create the AABB of (box1 in space of box0)
const Matrix3x3& mtx = _1in0.mRot;
float f = fabsf(mtx.m[0][0] * mExtents.x) + fabsf(mtx.m[1][0] * mExtents.y) + fabsf(mtx.m[2][0] * mExtents.z) - box.mExtents.x;
if(f > _1in0.mCenter.x) return FALSE;
if(-f < _1in0.mCenter.x) return FALSE;
f = fabsf(mtx.m[0][1] * mExtents.x) + fabsf(mtx.m[1][1] * mExtents.y) + fabsf(mtx.m[2][1] * mExtents.z) - box.mExtents.y;
if(f > _1in0.mCenter.y) return FALSE;
if(-f < _1in0.mCenter.y) return FALSE;
f = fabsf(mtx.m[0][2] * mExtents.x) + fabsf(mtx.m[1][2] * mExtents.y) + fabsf(mtx.m[2][2] * mExtents.z) - box.mExtents.z;
if(f > _1in0.mCenter.z) return FALSE;
if(-f < _1in0.mCenter.z) return FALSE;
return TRUE;
}

177
ode/OPCODE/Ice/IceOBB.h Normal file
View File

@ -0,0 +1,177 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains OBB-related code. (oriented bounding box)
* \file IceOBB.h
* \author Pierre Terdiman
* \date January, 13, 2000
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __ICEOBB_H__
#define __ICEOBB_H__
// Forward declarations
class LSS;
class ICEMATHS_API OBB
{
public:
//! Constructor
inline_ OBB() {}
//! Constructor
inline_ OBB(const Point& center, const Point& extents, const Matrix3x3& rot) : mCenter(center), mExtents(extents), mRot(rot) {}
//! Destructor
inline_ ~OBB() {}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Setups an empty OBB.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void SetEmpty()
{
mCenter.Zero();
mExtents.Set(MIN_FLOAT, MIN_FLOAT, MIN_FLOAT);
mRot.Identity();
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Tests if a point is contained within the OBB.
* \param p [in] the world point to test
* \return true if inside the OBB
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool ContainsPoint(const Point& p) const;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Builds an OBB from an AABB and a world transform.
* \param aabb [in] the aabb
* \param mat [in] the world transform
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void Create(const AABB& aabb, const Matrix4x4& mat);
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recomputes the OBB after an arbitrary transform by a 4x4 matrix.
* \param mtx [in] the transform matrix
* \param obb [out] the transformed OBB
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ void Rotate(const Matrix4x4& mtx, OBB& obb) const
{
// The extents remain constant
obb.mExtents = mExtents;
// The center gets x-formed
obb.mCenter = mCenter * mtx;
// Combine rotations
obb.mRot = mRot * Matrix3x3(mtx);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Checks the OBB is valid.
* \return true if the box is valid
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ BOOL IsValid() const
{
// Consistency condition for (Center, Extents) boxes: Extents >= 0.0f
if(mExtents.x < 0.0f) return FALSE;
if(mExtents.y < 0.0f) return FALSE;
if(mExtents.z < 0.0f) return FALSE;
return TRUE;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the obb planes.
* \param planes [out] 6 box planes
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool ComputePlanes(Plane* planes) const;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the obb points.
* \param pts [out] 8 box points
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool ComputePoints(Point* pts) const;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes vertex normals.
* \param pts [out] 8 box points
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool ComputeVertexNormals(Point* pts) const;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Returns edges.
* \return 24 indices (12 edges) indexing the list returned by ComputePoints()
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
const udword* GetEdges() const;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Returns local edge normals.
* \return edge normals in local space
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
const Point* GetLocalEdgeNormals() const;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Returns world edge normal
* \param edge_index [in] 0 <= edge index < 12
* \param world_normal [out] edge normal in world space
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void ComputeWorldEdgeNormal(udword edge_index, Point& world_normal) const;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes an LSS surrounding the OBB.
* \param lss [out] the LSS
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void ComputeLSS(LSS& lss) const;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Checks the OBB is inside another OBB.
* \param box [in] the other OBB
* \return TRUE if we're inside the other box
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
BOOL IsInside(const OBB& box) const;
inline_ const Point& GetCenter() const { return mCenter; }
inline_ const Point& GetExtents() const { return mExtents; }
inline_ const Matrix3x3& GetRot() const { return mRot; }
inline_ void GetRotatedExtents(Matrix3x3& extents) const
{
extents = mRot;
extents.Scale(mExtents);
}
Point mCenter; //!< B for Box
Point mExtents; //!< B for Bounding
Matrix3x3 mRot; //!< O for Oriented
// Orientation is stored in row-major format,
// i.e. rows = eigen vectors of the covariance matrix
};
#endif // __ICEOBB_H__

45
ode/OPCODE/Ice/IcePairs.h Normal file
View File

@ -0,0 +1,45 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains a simple pair class.
* \file IcePairs.h
* \author Pierre Terdiman
* \date January, 13, 2003
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __ICEPAIRS_H__
#define __ICEPAIRS_H__
//! A generic couple structure
struct ICECORE_API Pair
{
inline_ Pair() {}
inline_ Pair(udword i0, udword i1) : id0(i0), id1(i1) {}
udword id0; //!< First index of the pair
udword id1; //!< Second index of the pair
};
class ICECORE_API Pairs : private Container
{
public:
// Constructor / Destructor
Pairs() {}
~Pairs() {}
inline_ udword GetNbPairs() const { return GetNbEntries()>>1; }
inline_ const Pair* GetPairs() const { return (const Pair*)GetEntries(); }
inline_ const Pair* GetPair(udword i) const { return (const Pair*)&GetEntries()[i+i]; }
inline_ BOOL HasPairs() const { return IsNotEmpty(); }
inline_ void ResetPairs() { Reset(); }
inline_ void DeleteLastPair() { DeleteLastEntry(); DeleteLastEntry(); }
inline_ void AddPair(const Pair& p) { Add(p.id0).Add(p.id1); }
inline_ void AddPair(udword id0, udword id1) { Add(id0).Add(id1); }
};
#endif // __ICEPAIRS_H__

45
ode/OPCODE/Ice/IcePlane.cpp Normal file
View File

@ -0,0 +1,45 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for planes.
* \file IcePlane.cpp
* \author Pierre Terdiman
* \date April, 4, 2000
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Plane class.
* \class Plane
* \author Pierre Terdiman
* \version 1.0
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Precompiled Header
#include "Stdafx.h"
using namespace IceMaths;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the plane equation from 3 points.
* \param p0 [in] first point
* \param p1 [in] second point
* \param p2 [in] third point
* \return Self-reference
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Plane& Plane::Set(const Point& p0, const Point& p1, const Point& p2)
{
Point Edge0 = p1 - p0;
Point Edge1 = p2 - p0;
n = Edge0 ^ Edge1;
n.Normalize();
d = -(p0 | n);
return *this;
}

113
ode/OPCODE/Ice/IcePlane.h Normal file
View File

@ -0,0 +1,113 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for planes.
* \file IcePlane.h
* \author Pierre Terdiman
* \date April, 4, 2000
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __ICEPLANE_H__
#define __ICEPLANE_H__
#define PLANE_EPSILON (1.0e-7f)
class ICEMATHS_API Plane
{
public:
//! Constructor
inline_ Plane() { }
//! Constructor from a normal and a distance
inline_ Plane(float nx, float ny, float nz, float d) { Set(nx, ny, nz, d); }
//! Constructor from a point on the plane and a normal
inline_ Plane(const Point& p, const Point& n) { Set(p, n); }
//! Constructor from three points
inline_ Plane(const Point& p0, const Point& p1, const Point& p2) { Set(p0, p1, p2); }
//! Constructor from a normal and a distance
inline_ Plane(const Point& _n, float _d) { n = _n; d = _d; }
//! Copy constructor
inline_ Plane(const Plane& plane) : n(plane.n), d(plane.d) { }
//! Destructor
inline_ ~Plane() { }
inline_ Plane& Zero() { n.Zero(); d = 0.0f; return *this; }
inline_ Plane& Set(float nx, float ny, float nz, float _d) { n.Set(nx, ny, nz); d = _d; return *this; }
inline_ Plane& Set(const Point& p, const Point& _n) { n = _n; d = - p | _n; return *this; }
Plane& Set(const Point& p0, const Point& p1, const Point& p2);
inline_ float Distance(const Point& p) const { return (p | n) + d; }
inline_ bool Belongs(const Point& p) const { return fabsf(Distance(p)) < PLANE_EPSILON; }
inline_ void Normalize()
{
float Denom = 1.0f / n.Magnitude();
n.x *= Denom;
n.y *= Denom;
n.z *= Denom;
d *= Denom;
}
public:
// Members
Point n; //!< The normal to the plane
float d; //!< The distance from the origin
// Cast operators
inline_ operator Point() const { return n; }
inline_ operator HPoint() const { return HPoint(n, d); }
// Arithmetic operators
inline_ Plane operator*(const Matrix4x4& m) const
{
// Old code from Irion. Kept for reference.
Plane Ret(*this);
return Ret *= m;
}
inline_ Plane& operator*=(const Matrix4x4& m)
{
// Old code from Irion. Kept for reference.
Point n2 = HPoint(n, 0.0f) * m;
d = -((Point) (HPoint( -d*n, 1.0f ) * m) | n2);
n = n2;
return *this;
}
};
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Transforms a plane by a 4x4 matrix. Same as Plane * Matrix4x4 operator, but faster.
* \param transformed [out] transformed plane
* \param plane [in] source plane
* \param transform [in] transform matrix
* \warning the plane normal must be unit-length
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ void TransformPlane(Plane& transformed, const Plane& plane, const Matrix4x4& transform)
{
// Rotate the normal using the rotation part of the 4x4 matrix
transformed.n = plane.n * Matrix3x3(transform);
// Compute new d
transformed.d = plane.d - (Point(transform.GetTrans())|transformed.n);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Transforms a plane by a 4x4 matrix. Same as Plane * Matrix4x4 operator, but faster.
* \param plane [in/out] source plane (transformed on return)
* \param transform [in] transform matrix
* \warning the plane normal must be unit-length
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ void TransformPlane(Plane& plane, const Matrix4x4& transform)
{
// Rotate the normal using the rotation part of the 4x4 matrix
plane.n *= Matrix3x3(transform);
// Compute new d
plane.d -= Point(transform.GetTrans())|plane.n;
}
#endif // __ICEPLANE_H__

193
ode/OPCODE/Ice/IcePoint.cpp Normal file
View File

@ -0,0 +1,193 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for 3D vectors.
* \file IcePoint.cpp
* \author Pierre Terdiman
* \date April, 4, 2000
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* 3D point.
*
* The name is "Point" instead of "Vector" since a vector is N-dimensional, whereas a point is an implicit "vector of dimension 3".
* So the choice was between "Point" and "Vector3", the first one looked better (IMHO).
*
* Some people, then, use a typedef to handle both points & vectors using the same class: typedef Point Vector3;
* This is bad since it opens the door to a lot of confusion while reading the code. I know it may sounds weird but check this out:
*
* \code
* Point P0,P1 = some 3D points;
* Point Delta = P1 - P0;
* \endcode
*
* This compiles fine, although you should have written:
*
* \code
* Point P0,P1 = some 3D points;
* Vector3 Delta = P1 - P0;
* \endcode
*
* Subtle things like this are not caught at compile-time, and when you find one in the code, you never know whether it's a mistake
* from the author or something you don't get.
*
* One way to handle it at compile-time would be to use different classes for Point & Vector3, only overloading operator "-" for vectors.
* But then, you get a lot of redundant code in thoses classes, and basically it's really a lot of useless work.
*
* Another way would be to use homogeneous points: w=1 for points, w=0 for vectors. That's why the HPoint class exists. Now, to store
* your model's vertices and in most cases, you really want to use Points to save ram.
*
* \class Point
* \author Pierre Terdiman
* \version 1.0
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Precompiled Header
#include "Stdafx.h"
using namespace IceMaths;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Creates a positive unit random vector.
* \return Self-reference
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Point& Point::PositiveUnitRandomVector()
{
x = UnitRandomFloat();
y = UnitRandomFloat();
z = UnitRandomFloat();
Normalize();
return *this;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Creates a unit random vector.
* \return Self-reference
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Point& Point::UnitRandomVector()
{
x = UnitRandomFloat() - 0.5f;
y = UnitRandomFloat() - 0.5f;
z = UnitRandomFloat() - 0.5f;
Normalize();
return *this;
}
// Cast operator
// WARNING: not inlined
Point::operator HPoint() const { return HPoint(x, y, z, 0.0f); }
Point& Point::Refract(const Point& eye, const Point& n, float refractindex, Point& refracted)
{
// Point EyePt = eye position
// Point p = current vertex
// Point n = vertex normal
// Point rv = refracted vector
// Eye vector - doesn't need to be normalized
Point Env;
Env.x = eye.x - x;
Env.y = eye.y - y;
Env.z = eye.z - z;
float NDotE = n|Env;
float NDotN = n|n;
NDotE /= refractindex;
// Refracted vector
refracted = n*NDotE - Env*NDotN;
return *this;
}
Point& Point::ProjectToPlane(const Plane& p)
{
*this-= (p.d + (*this|p.n))*p.n;
return *this;
}
void Point::ProjectToScreen(float halfrenderwidth, float halfrenderheight, const Matrix4x4& mat, HPoint& projected) const
{
projected = HPoint(x, y, z, 1.0f) * mat;
projected.w = 1.0f / projected.w;
projected.x*=projected.w;
projected.y*=projected.w;
projected.z*=projected.w;
projected.x *= halfrenderwidth; projected.x += halfrenderwidth;
projected.y *= -halfrenderheight; projected.y += halfrenderheight;
}
void Point::SetNotUsed()
{
// We use a particular integer pattern : 0xffffffff everywhere. This is a NAN.
IR(x) = 0xffffffff;
IR(y) = 0xffffffff;
IR(z) = 0xffffffff;
}
BOOL Point::IsNotUsed() const
{
if(IR(x)!=0xffffffff) return FALSE;
if(IR(y)!=0xffffffff) return FALSE;
if(IR(z)!=0xffffffff) return FALSE;
return TRUE;
}
Point& Point::Mult(const Matrix3x3& mat, const Point& a)
{
x = a.x * mat.m[0][0] + a.y * mat.m[0][1] + a.z * mat.m[0][2];
y = a.x * mat.m[1][0] + a.y * mat.m[1][1] + a.z * mat.m[1][2];
z = a.x * mat.m[2][0] + a.y * mat.m[2][1] + a.z * mat.m[2][2];
return *this;
}
Point& Point::Mult2(const Matrix3x3& mat1, const Point& a1, const Matrix3x3& mat2, const Point& a2)
{
x = a1.x * mat1.m[0][0] + a1.y * mat1.m[0][1] + a1.z * mat1.m[0][2] + a2.x * mat2.m[0][0] + a2.y * mat2.m[0][1] + a2.z * mat2.m[0][2];
y = a1.x * mat1.m[1][0] + a1.y * mat1.m[1][1] + a1.z * mat1.m[1][2] + a2.x * mat2.m[1][0] + a2.y * mat2.m[1][1] + a2.z * mat2.m[1][2];
z = a1.x * mat1.m[2][0] + a1.y * mat1.m[2][1] + a1.z * mat1.m[2][2] + a2.x * mat2.m[2][0] + a2.y * mat2.m[2][1] + a2.z * mat2.m[2][2];
return *this;
}
Point& Point::Mac(const Matrix3x3& mat, const Point& a)
{
x += a.x * mat.m[0][0] + a.y * mat.m[0][1] + a.z * mat.m[0][2];
y += a.x * mat.m[1][0] + a.y * mat.m[1][1] + a.z * mat.m[1][2];
z += a.x * mat.m[2][0] + a.y * mat.m[2][1] + a.z * mat.m[2][2];
return *this;
}
Point& Point::TransMult(const Matrix3x3& mat, const Point& a)
{
x = a.x * mat.m[0][0] + a.y * mat.m[1][0] + a.z * mat.m[2][0];
y = a.x * mat.m[0][1] + a.y * mat.m[1][1] + a.z * mat.m[2][1];
z = a.x * mat.m[0][2] + a.y * mat.m[1][2] + a.z * mat.m[2][2];
return *this;
}
Point& Point::Transform(const Point& r, const Matrix3x3& rotpos, const Point& linpos)
{
x = r.x * rotpos.m[0][0] + r.y * rotpos.m[0][1] + r.z * rotpos.m[0][2] + linpos.x;
y = r.x * rotpos.m[1][0] + r.y * rotpos.m[1][1] + r.z * rotpos.m[1][2] + linpos.y;
z = r.x * rotpos.m[2][0] + r.y * rotpos.m[2][1] + r.z * rotpos.m[2][2] + linpos.z;
return *this;
}
Point& Point::InvTransform(const Point& r, const Matrix3x3& rotpos, const Point& linpos)
{
float sx = r.x - linpos.x;
float sy = r.y - linpos.y;
float sz = r.z - linpos.z;
x = sx * rotpos.m[0][0] + sy * rotpos.m[1][0] + sz * rotpos.m[2][0];
y = sx * rotpos.m[0][1] + sy * rotpos.m[1][1] + sz * rotpos.m[2][1];
z = sx * rotpos.m[0][2] + sy * rotpos.m[1][2] + sz * rotpos.m[2][2];
return *this;
}

528
ode/OPCODE/Ice/IcePoint.h Normal file
View File

@ -0,0 +1,528 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for 3D vectors.
* \file IcePoint.h
* \author Pierre Terdiman
* \date April, 4, 2000
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __ICEPOINT_H__
#define __ICEPOINT_H__
// Forward declarations
class HPoint;
class Plane;
class Matrix3x3;
class Matrix4x4;
#define CROSS2D(a, b) (a.x*b.y - b.x*a.y)
const float EPSILON2 = 1.0e-20f;
class ICEMATHS_API Point
{
public:
//! Empty constructor
inline_ Point() {}
//! Constructor from a single float
// inline_ Point(float val) : x(val), y(val), z(val) {}
// Removed since it introduced the nasty "Point T = *Matrix4x4.GetTrans();" bug.......
//! Constructor from floats
inline_ Point(float xx, float yy, float zz) : x(xx), y(yy), z(zz) {}
//! Constructor from array
inline_ Point(const float f[3]) : x(f[X]), y(f[Y]), z(f[Z]) {}
//! Copy constructor
inline_ Point(const Point& p) : x(p.x), y(p.y), z(p.z) {}
//! Destructor
inline_ ~Point() {}
//! Clears the vector
inline_ Point& Zero() { x = y = z = 0.0f; return *this; }
//! + infinity
inline_ Point& SetPlusInfinity() { x = y = z = MAX_FLOAT; return *this; }
//! - infinity
inline_ Point& SetMinusInfinity() { x = y = z = MIN_FLOAT; return *this; }
//! Sets positive unit random vector
Point& PositiveUnitRandomVector();
//! Sets unit random vector
Point& UnitRandomVector();
//! Assignment from values
inline_ Point& Set(float xx, float yy, float zz) { x = xx; y = yy; z = zz; return *this; }
//! Assignment from array
inline_ Point& Set(const float f[3]) { x = f[X]; y = f[Y]; z = f[Z]; return *this; }
//! Assignment from another point
inline_ Point& Set(const Point& src) { x = src.x; y = src.y; z = src.z; return *this; }
//! Adds a vector
inline_ Point& Add(const Point& p) { x += p.x; y += p.y; z += p.z; return *this; }
//! Adds a vector
inline_ Point& Add(float xx, float yy, float zz) { x += xx; y += yy; z += zz; return *this; }
//! Adds a vector
inline_ Point& Add(const float f[3]) { x += f[X]; y += f[Y]; z += f[Z]; return *this; }
//! Adds vectors
inline_ Point& Add(const Point& p, const Point& q) { x = p.x+q.x; y = p.y+q.y; z = p.z+q.z; return *this; }
//! Subtracts a vector
inline_ Point& Sub(const Point& p) { x -= p.x; y -= p.y; z -= p.z; return *this; }
//! Subtracts a vector
inline_ Point& Sub(float xx, float yy, float zz) { x -= xx; y -= yy; z -= zz; return *this; }
//! Subtracts a vector
inline_ Point& Sub(const float f[3]) { x -= f[X]; y -= f[Y]; z -= f[Z]; return *this; }
//! Subtracts vectors
inline_ Point& Sub(const Point& p, const Point& q) { x = p.x-q.x; y = p.y-q.y; z = p.z-q.z; return *this; }
//! this = -this
inline_ Point& Neg() { x = -x; y = -y; z = -z; return *this; }
//! this = -a
inline_ Point& Neg(const Point& a) { x = -a.x; y = -a.y; z = -a.z; return *this; }
//! Multiplies by a scalar
inline_ Point& Mult(float s) { x *= s; y *= s; z *= s; return *this; }
//! this = a * scalar
inline_ Point& Mult(const Point& a, float scalar)
{
x = a.x * scalar;
y = a.y * scalar;
z = a.z * scalar;
return *this;
}
//! this = a + b * scalar
inline_ Point& Mac(const Point& a, const Point& b, float scalar)
{
x = a.x + b.x * scalar;
y = a.y + b.y * scalar;
z = a.z + b.z * scalar;
return *this;
}
//! this = this + a * scalar
inline_ Point& Mac(const Point& a, float scalar)
{
x += a.x * scalar;
y += a.y * scalar;
z += a.z * scalar;
return *this;
}
//! this = a - b * scalar
inline_ Point& Msc(const Point& a, const Point& b, float scalar)
{
x = a.x - b.x * scalar;
y = a.y - b.y * scalar;
z = a.z - b.z * scalar;
return *this;
}
//! this = this - a * scalar
inline_ Point& Msc(const Point& a, float scalar)
{
x -= a.x * scalar;
y -= a.y * scalar;
z -= a.z * scalar;
return *this;
}
//! this = a + b * scalarb + c * scalarc
inline_ Point& Mac2(const Point& a, const Point& b, float scalarb, const Point& c, float scalarc)
{
x = a.x + b.x * scalarb + c.x * scalarc;
y = a.y + b.y * scalarb + c.y * scalarc;
z = a.z + b.z * scalarb + c.z * scalarc;
return *this;
}
//! this = a - b * scalarb - c * scalarc
inline_ Point& Msc2(const Point& a, const Point& b, float scalarb, const Point& c, float scalarc)
{
x = a.x - b.x * scalarb - c.x * scalarc;
y = a.y - b.y * scalarb - c.y * scalarc;
z = a.z - b.z * scalarb - c.z * scalarc;
return *this;
}
//! this = mat * a
inline_ Point& Mult(const Matrix3x3& mat, const Point& a);
//! this = mat1 * a1 + mat2 * a2
inline_ Point& Mult2(const Matrix3x3& mat1, const Point& a1, const Matrix3x3& mat2, const Point& a2);
//! this = this + mat * a
inline_ Point& Mac(const Matrix3x3& mat, const Point& a);
//! this = transpose(mat) * a
inline_ Point& TransMult(const Matrix3x3& mat, const Point& a);
//! Linear interpolate between two vectors: this = a + t * (b - a)
inline_ Point& Lerp(const Point& a, const Point& b, float t)
{
x = a.x + t * (b.x - a.x);
y = a.y + t * (b.y - a.y);
z = a.z + t * (b.z - a.z);
return *this;
}
//! Hermite interpolate between p1 and p2. p0 and p3 are used for finding gradient at p1 and p2.
//! this = p0 * (2t^2 - t^3 - t)/2
//! + p1 * (3t^3 - 5t^2 + 2)/2
//! + p2 * (4t^2 - 3t^3 + t)/2
//! + p3 * (t^3 - t^2)/2
inline_ Point& Herp(const Point& p0, const Point& p1, const Point& p2, const Point& p3, float t)
{
float t2 = t * t;
float t3 = t2 * t;
float kp0 = (2.0f * t2 - t3 - t) * 0.5f;
float kp1 = (3.0f * t3 - 5.0f * t2 + 2.0f) * 0.5f;
float kp2 = (4.0f * t2 - 3.0f * t3 + t) * 0.5f;
float kp3 = (t3 - t2) * 0.5f;
x = p0.x * kp0 + p1.x * kp1 + p2.x * kp2 + p3.x * kp3;
y = p0.y * kp0 + p1.y * kp1 + p2.y * kp2 + p3.y * kp3;
z = p0.z * kp0 + p1.z * kp1 + p2.z * kp2 + p3.z * kp3;
return *this;
}
//! this = rotpos * r + linpos
inline_ Point& Transform(const Point& r, const Matrix3x3& rotpos, const Point& linpos);
//! this = trans(rotpos) * (r - linpos)
inline_ Point& InvTransform(const Point& r, const Matrix3x3& rotpos, const Point& linpos);
//! Returns MIN(x, y, z);
inline_ float Min() const { return MIN(x, MIN(y, z)); }
//! Returns MAX(x, y, z);
inline_ float Max() const { return MAX(x, MAX(y, z)); }
//! Sets each element to be componentwise minimum
inline_ Point& Min(const Point& p) { x = MIN(x, p.x); y = MIN(y, p.y); z = MIN(z, p.z); return *this; }
//! Sets each element to be componentwise maximum
inline_ Point& Max(const Point& p) { x = MAX(x, p.x); y = MAX(y, p.y); z = MAX(z, p.z); return *this; }
//! Clamps each element
inline_ Point& Clamp(float min, float max)
{
if(x<min) x=min; if(x>max) x=max;
if(y<min) y=min; if(y>max) y=max;
if(z<min) z=min; if(z>max) z=max;
return *this;
}
//! Computes square magnitude
inline_ float SquareMagnitude() const { return x*x + y*y + z*z; }
//! Computes magnitude
inline_ float Magnitude() const { return sqrtf(x*x + y*y + z*z); }
//! Computes volume
inline_ float Volume() const { return x * y * z; }
//! Checks the point is near zero
inline_ bool ApproxZero() const { return SquareMagnitude() < EPSILON2; }
//! Tests for exact zero vector
inline_ BOOL IsZero() const
{
if(IR(x) || IR(y) || IR(z)) return FALSE;
return TRUE;
}
//! Checks point validity
inline_ BOOL IsValid() const
{
if(!IsValidFloat(x)) return FALSE;
if(!IsValidFloat(y)) return FALSE;
if(!IsValidFloat(z)) return FALSE;
return TRUE;
}
//! Slighty moves the point
void Tweak(udword coord_mask, udword tweak_mask)
{
if(coord_mask&1) { udword Dummy = IR(x); Dummy^=tweak_mask; x = FR(Dummy); }
if(coord_mask&2) { udword Dummy = IR(y); Dummy^=tweak_mask; y = FR(Dummy); }
if(coord_mask&4) { udword Dummy = IR(z); Dummy^=tweak_mask; z = FR(Dummy); }
}
#define TWEAKMASK 0x3fffff
#define TWEAKNOTMASK ~TWEAKMASK
//! Slighty moves the point out
inline_ void TweakBigger()
{
udword Dummy = (IR(x)&TWEAKNOTMASK); if(!IS_NEGATIVE_FLOAT(x)) Dummy+=TWEAKMASK+1; x = FR(Dummy);
Dummy = (IR(y)&TWEAKNOTMASK); if(!IS_NEGATIVE_FLOAT(y)) Dummy+=TWEAKMASK+1; y = FR(Dummy);
Dummy = (IR(z)&TWEAKNOTMASK); if(!IS_NEGATIVE_FLOAT(z)) Dummy+=TWEAKMASK+1; z = FR(Dummy);
}
//! Slighty moves the point in
inline_ void TweakSmaller()
{
udword Dummy = (IR(x)&TWEAKNOTMASK); if(IS_NEGATIVE_FLOAT(x)) Dummy+=TWEAKMASK+1; x = FR(Dummy);
Dummy = (IR(y)&TWEAKNOTMASK); if(IS_NEGATIVE_FLOAT(y)) Dummy+=TWEAKMASK+1; y = FR(Dummy);
Dummy = (IR(z)&TWEAKNOTMASK); if(IS_NEGATIVE_FLOAT(z)) Dummy+=TWEAKMASK+1; z = FR(Dummy);
}
//! Normalizes the vector
inline_ Point& Normalize()
{
float M = x*x + y*y + z*z;
if(M)
{
M = 1.0f / sqrtf(M);
x *= M;
y *= M;
z *= M;
}
return *this;
}
//! Sets vector length
inline_ Point& SetLength(float length)
{
float NewLength = length / Magnitude();
x *= NewLength;
y *= NewLength;
z *= NewLength;
return *this;
}
//! Clamps vector length
inline_ Point& ClampLength(float limit_length)
{
if(limit_length>=0.0f) // Magnitude must be positive
{
float CurrentSquareLength = SquareMagnitude();
if(CurrentSquareLength > limit_length * limit_length)
{
float Coeff = limit_length / sqrtf(CurrentSquareLength);
x *= Coeff;
y *= Coeff;
z *= Coeff;
}
}
return *this;
}
//! Computes distance to another point
inline_ float Distance(const Point& b) const
{
return sqrtf((x - b.x)*(x - b.x) + (y - b.y)*(y - b.y) + (z - b.z)*(z - b.z));
}
//! Computes square distance to another point
inline_ float SquareDistance(const Point& b) const
{
return ((x - b.x)*(x - b.x) + (y - b.y)*(y - b.y) + (z - b.z)*(z - b.z));
}
//! Dot product dp = this|a
inline_ float Dot(const Point& p) const { return p.x * x + p.y * y + p.z * z; }
//! Cross product this = a x b
inline_ Point& Cross(const Point& a, const Point& b)
{
x = a.y * b.z - a.z * b.y;
y = a.z * b.x - a.x * b.z;
z = a.x * b.y - a.y * b.x;
return *this;
}
//! Vector code ( bitmask = sign(z) | sign(y) | sign(x) )
inline_ udword VectorCode() const
{
return (IR(x)>>31) | ((IR(y)&SIGN_BITMASK)>>30) | ((IR(z)&SIGN_BITMASK)>>29);
}
//! Returns largest axis
inline_ PointComponent LargestAxis() const
{
const float* Vals = &x;
PointComponent m = X;
if(Vals[Y] > Vals[m]) m = Y;
if(Vals[Z] > Vals[m]) m = Z;
return m;
}
//! Returns closest axis
inline_ PointComponent ClosestAxis() const
{
const float* Vals = &x;
PointComponent m = X;
if(AIR(Vals[Y]) > AIR(Vals[m])) m = Y;
if(AIR(Vals[Z]) > AIR(Vals[m])) m = Z;
return m;
}
//! Returns smallest axis
inline_ PointComponent SmallestAxis() const
{
const float* Vals = &x;
PointComponent m = X;
if(Vals[Y] < Vals[m]) m = Y;
if(Vals[Z] < Vals[m]) m = Z;
return m;
}
//! Refracts the point
Point& Refract(const Point& eye, const Point& n, float refractindex, Point& refracted);
//! Projects the point onto a plane
Point& ProjectToPlane(const Plane& p);
//! Projects the point onto the screen
void ProjectToScreen(float halfrenderwidth, float halfrenderheight, const Matrix4x4& mat, HPoint& projected) const;
//! Unfolds the point onto a plane according to edge(a,b)
Point& Unfold(Plane& p, Point& a, Point& b);
//! Hash function from Ville Miettinen
inline_ udword GetHashValue() const
{
const udword* h = (const udword*)(this);
udword f = (h[0]+h[1]*11-(h[2]*17)) & 0x7fffffff; // avoid problems with +-0
return (f>>22)^(f>>12)^(f);
}
//! Stuff magic values in the point, marking it as explicitely not used.
void SetNotUsed();
//! Checks the point is marked as not used
BOOL IsNotUsed() const;
// Arithmetic operators
//! Unary operator for Point Negate = - Point
inline_ Point operator-() const { return Point(-x, -y, -z); }
//! Operator for Point Plus = Point + Point.
inline_ Point operator+(const Point& p) const { return Point(x + p.x, y + p.y, z + p.z); }
//! Operator for Point Minus = Point - Point.
inline_ Point operator-(const Point& p) const { return Point(x - p.x, y - p.y, z - p.z); }
//! Operator for Point Mul = Point * Point.
inline_ Point operator*(const Point& p) const { return Point(x * p.x, y * p.y, z * p.z); }
//! Operator for Point Scale = Point * float.
inline_ Point operator*(float s) const { return Point(x * s, y * s, z * s ); }
//! Operator for Point Scale = float * Point.
inline_ friend Point operator*(float s, const Point& p) { return Point(s * p.x, s * p.y, s * p.z); }
//! Operator for Point Div = Point / Point.
inline_ Point operator/(const Point& p) const { return Point(x / p.x, y / p.y, z / p.z); }
//! Operator for Point Scale = Point / float.
inline_ Point operator/(float s) const { s = 1.0f / s; return Point(x * s, y * s, z * s); }
//! Operator for Point Scale = float / Point.
inline_ friend Point operator/(float s, const Point& p) { return Point(s / p.x, s / p.y, s / p.z); }
//! Operator for float DotProd = Point | Point.
inline_ float operator|(const Point& p) const { return x*p.x + y*p.y + z*p.z; }
//! Operator for Point VecProd = Point ^ Point.
inline_ Point operator^(const Point& p) const
{
return Point(
y * p.z - z * p.y,
z * p.x - x * p.z,
x * p.y - y * p.x );
}
//! Operator for Point += Point.
inline_ Point& operator+=(const Point& p) { x += p.x; y += p.y; z += p.z; return *this; }
//! Operator for Point += float.
inline_ Point& operator+=(float s) { x += s; y += s; z += s; return *this; }
//! Operator for Point -= Point.
inline_ Point& operator-=(const Point& p) { x -= p.x; y -= p.y; z -= p.z; return *this; }
//! Operator for Point -= float.
inline_ Point& operator-=(float s) { x -= s; y -= s; z -= s; return *this; }
//! Operator for Point *= Point.
inline_ Point& operator*=(const Point& p) { x *= p.x; y *= p.y; z *= p.z; return *this; }
//! Operator for Point *= float.
inline_ Point& operator*=(float s) { x *= s; y *= s; z *= s; return *this; }
//! Operator for Point /= Point.
inline_ Point& operator/=(const Point& p) { x /= p.x; y /= p.y; z /= p.z; return *this; }
//! Operator for Point /= float.
inline_ Point& operator/=(float s) { s = 1.0f/s; x *= s; y *= s; z *= s; return *this; }
// Logical operators
//! Operator for "if(Point==Point)"
inline_ bool operator==(const Point& p) const { return ( (IR(x)==IR(p.x))&&(IR(y)==IR(p.y))&&(IR(z)==IR(p.z))); }
//! Operator for "if(Point!=Point)"
inline_ bool operator!=(const Point& p) const { return ( (IR(x)!=IR(p.x))||(IR(y)!=IR(p.y))||(IR(z)!=IR(p.z))); }
// Arithmetic operators
//! Operator for Point Mul = Point * Matrix3x3.
inline_ Point operator*(const Matrix3x3& mat) const
{
class ShadowMatrix3x3{ public: float m[3][3]; }; // To allow inlining
const ShadowMatrix3x3* Mat = (const ShadowMatrix3x3*)&mat;
return Point(
x * Mat->m[0][0] + y * Mat->m[1][0] + z * Mat->m[2][0],
x * Mat->m[0][1] + y * Mat->m[1][1] + z * Mat->m[2][1],
x * Mat->m[0][2] + y * Mat->m[1][2] + z * Mat->m[2][2] );
}
//! Operator for Point Mul = Point * Matrix4x4.
inline_ Point operator*(const Matrix4x4& mat) const
{
class ShadowMatrix4x4{ public: float m[4][4]; }; // To allow inlining
const ShadowMatrix4x4* Mat = (const ShadowMatrix4x4*)&mat;
return Point(
x * Mat->m[0][0] + y * Mat->m[1][0] + z * Mat->m[2][0] + Mat->m[3][0],
x * Mat->m[0][1] + y * Mat->m[1][1] + z * Mat->m[2][1] + Mat->m[3][1],
x * Mat->m[0][2] + y * Mat->m[1][2] + z * Mat->m[2][2] + Mat->m[3][2]);
}
//! Operator for Point *= Matrix3x3.
inline_ Point& operator*=(const Matrix3x3& mat)
{
class ShadowMatrix3x3{ public: float m[3][3]; }; // To allow inlining
const ShadowMatrix3x3* Mat = (const ShadowMatrix3x3*)&mat;
float xp = x * Mat->m[0][0] + y * Mat->m[1][0] + z * Mat->m[2][0];
float yp = x * Mat->m[0][1] + y * Mat->m[1][1] + z * Mat->m[2][1];
float zp = x * Mat->m[0][2] + y * Mat->m[1][2] + z * Mat->m[2][2];
x = xp; y = yp; z = zp;
return *this;
}
//! Operator for Point *= Matrix4x4.
inline_ Point& operator*=(const Matrix4x4& mat)
{
class ShadowMatrix4x4{ public: float m[4][4]; }; // To allow inlining
const ShadowMatrix4x4* Mat = (const ShadowMatrix4x4*)&mat;
float xp = x * Mat->m[0][0] + y * Mat->m[1][0] + z * Mat->m[2][0] + Mat->m[3][0];
float yp = x * Mat->m[0][1] + y * Mat->m[1][1] + z * Mat->m[2][1] + Mat->m[3][1];
float zp = x * Mat->m[0][2] + y * Mat->m[1][2] + z * Mat->m[2][2] + Mat->m[3][2];
x = xp; y = yp; z = zp;
return *this;
}
// Cast operators
//! Cast a Point to a HPoint. w is set to zero.
operator HPoint() const;
inline_ operator const float*() const { return &x; }
inline_ operator float*() { return &x; }
public:
float x, y, z;
};
FUNCTION ICEMATHS_API void Normalize1(Point& a);
FUNCTION ICEMATHS_API void Normalize2(Point& a);
#endif //__ICEPOINT_H__

132
ode/OPCODE/Ice/IcePreprocessor.h Normal file
View File

@ -0,0 +1,132 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains preprocessor stuff. This should be the first included header.
* \file IcePreprocessor.h
* \author Pierre Terdiman
* \date April, 4, 2000
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __ICEPREPROCESSOR_H__
#define __ICEPREPROCESSOR_H__
// Check platform
#if defined( _WIN32 ) || defined( WIN32 )
// #pragma message("Compiling on Windows...")
#define PLATFORM_WINDOWS
#else
// don't issue pragmas on unknown platforms
// #pragma message("Compiling on unknown platform...")
#endif
// Check compiler
#if defined(_MSC_VER)
// #pragma message("Compiling with VC++...")
#define COMPILER_VISUAL_CPP
#else
// don't issue pragmas on unknown platforms
// #pragma message("Compiling with unknown compiler...")
#endif
// Check compiler options. If this file is included in user-apps, this
// shouldn't be needed, so that they can use what they like best.
#ifndef ICE_DONT_CHECK_COMPILER_OPTIONS
#ifdef COMPILER_VISUAL_CPP
#if defined(_CHAR_UNSIGNED)
#endif
#if defined(_CPPRTTI)
#error Please disable RTTI...
#endif
#if defined(_CPPUNWIND)
#error Please disable exceptions...
#endif
#if defined(_MT)
// Multithreading
#endif
#endif
#endif
// Check debug mode
#ifdef DEBUG // May be defined instead of _DEBUG. Let's fix it.
#ifndef _DEBUG
#define _DEBUG
#endif
#endif
#ifdef _DEBUG
// Here you may define items for debug builds
#endif
#ifndef THIS_FILE
#define THIS_FILE __FILE__
#endif
#ifndef ICE_NO_DLL
#ifdef ICECORE_EXPORTS
#define ICECORE_API __declspec(dllexport)
#else
#define ICECORE_API __declspec(dllimport)
#endif
#else
#define ICECORE_API
#endif
// Don't override new/delete
// #define DEFAULT_NEWDELETE
#define DONT_TRACK_MEMORY_LEAKS
#define FUNCTION extern "C"
// Cosmetic stuff [mainly useful with multiple inheritance]
#define override(base_class) virtual
// Our own inline keyword, so that:
// - we can switch to __forceinline to check it's really better or not
// - we can remove __forceinline if the compiler doesn't support it
// #define inline_ __forceinline
// #define inline_ inline
// Contributed by Bruce Mitchener
#if defined(COMPILER_VISUAL_CPP)
#define inline_ __forceinline
// #define inline_ inline
#elif defined(__GNUC__) && __GNUC__ < 3
#define inline_ inline
#elif defined(__GNUC__)
#define inline_ inline __attribute__ ((always_inline))
#else
#define inline_ inline
#endif
// Down the hatch
#ifdef _MSC_VER
#pragma inline_depth( 255 )
#endif
#ifdef COMPILER_VISUAL_CPP
#pragma intrinsic(memcmp)
#pragma intrinsic(memcpy)
#pragma intrinsic(memset)
#pragma intrinsic(strcat)
#pragma intrinsic(strcmp)
#pragma intrinsic(strcpy)
#pragma intrinsic(strlen)
#pragma intrinsic(abs)
#pragma intrinsic(labs)
#endif
// ANSI compliance
#ifdef _DEBUG
// Remove painful warning in debug
inline_ bool __False__(){ return false; }
#define for if(__False__()){} else for
#else
#define for if(0){} else for
#endif
#endif // __ICEPREPROCESSOR_H__

35
ode/OPCODE/Ice/IceRandom.cpp Normal file
View File

@ -0,0 +1,35 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for random generators.
* \file IceRandom.cpp
* \author Pierre Terdiman
* \date August, 9, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Precompiled Header
#include "Stdafx.h"
using namespace IceCore;
void IceCore:: SRand(udword seed)
{
srand(seed);
}
udword IceCore::Rand()
{
return rand();
}
static BasicRandom gRandomGenerator(42);
udword IceCore::GetRandomIndex(udword max_index)
{
// We don't use rand() since it's limited to RAND_MAX
udword Index = gRandomGenerator.Randomize();
return Index % max_index;
}

42
ode/OPCODE/Ice/IceRandom.h Normal file
View File

@ -0,0 +1,42 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for random generators.
* \file IceRandom.h
* \author Pierre Terdiman
* \date August, 9, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __ICERANDOM_H__
#define __ICERANDOM_H__
FUNCTION ICECORE_API void SRand(udword seed);
FUNCTION ICECORE_API udword Rand();
//! Returns a unit random floating-point value
inline_ float UnitRandomFloat() { return float(Rand()) * ONE_OVER_RAND_MAX; }
//! Returns a random index so that 0<= index < max_index
ICECORE_API udword GetRandomIndex(udword max_index);
class ICECORE_API BasicRandom
{
public:
//! Constructor
inline_ BasicRandom(udword seed=0) : mRnd(seed) {}
//! Destructor
inline_ ~BasicRandom() {}
inline_ void SetSeed(udword seed) { mRnd = seed; }
inline_ udword GetCurrentValue() const { return mRnd; }
inline_ udword Randomize() { mRnd = mRnd * 2147001325 + 715136305; return mRnd; }
private:
udword mRnd;
};
#endif // __ICERANDOM_H__

84
ode/OPCODE/Ice/IceRay.cpp Normal file
View File

@ -0,0 +1,84 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for rays.
* \file IceRay.cpp
* \author Pierre Terdiman
* \date April, 4, 2000
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Ray class.
* A ray is a half-line P(t) = mOrig + mDir * t, with 0 <= t <= +infinity
* \class Ray
* \author Pierre Terdiman
* \version 1.0
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
O = Origin = impact point
i = normalized vector along the x axis
j = normalized vector along the y axis = actually the normal vector in O
D = Direction vector, norm |D| = 1
N = Projection of D on y axis, norm |N| = normal reaction
T = Projection of D on x axis, norm |T| = tangential reaction
R = Reflexion vector
^y
|
|
|
_ _ _| _ _ _
* * *|
\ | /
\ |N / |
R\ | /D
\ | / |
\ | /
_________\|/______*_______>x
O T
Let define theta = angle between D and N. Then cos(theta) = |N| / |D| = |N| since D is normalized.
j|D = |j|*|D|*cos(theta) => |N| = j|D
Then we simply have:
D = N + T
To compute tangential reaction :
T = D - N
To compute reflexion vector :
R = N - T = N - (D-N) = 2*N - D
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Precompiled Header
#include "Stdafx.h"
using namespace IceMaths;
float Ray::SquareDistance(const Point& point, float* t) const
{
Point Diff = point - mOrig;
float fT = Diff | mDir;
if(fT<=0.0f)
{
fT = 0.0f;
}
else
{
fT /= mDir.SquareMagnitude();
Diff -= fT*mDir;
}
if(t) *t = fT;
return Diff.SquareMagnitude();
}

98
ode/OPCODE/Ice/IceRay.h Normal file
View File

@ -0,0 +1,98 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for rays.
* \file IceRay.h
* \author Pierre Terdiman
* \date April, 4, 2000
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __ICERAY_H__
#define __ICERAY_H__
class ICEMATHS_API Ray
{
public:
//! Constructor
inline_ Ray() {}
//! Constructor
inline_ Ray(const Point& orig, const Point& dir) : mOrig(orig), mDir(dir) {}
//! Copy constructor
inline_ Ray(const Ray& ray) : mOrig(ray.mOrig), mDir(ray.mDir) {}
//! Destructor
inline_ ~Ray() {}
float SquareDistance(const Point& point, float* t=null) const;
inline_ float Distance(const Point& point, float* t=null) const { return sqrtf(SquareDistance(point, t)); }
Point mOrig; //!< Ray origin
Point mDir; //!< Normalized direction
};
inline_ void ComputeReflexionVector(Point& reflected, const Point& incoming_dir, const Point& outward_normal)
{
reflected = incoming_dir - outward_normal * 2.0f * (incoming_dir|outward_normal);
}
inline_ void ComputeReflexionVector(Point& reflected, const Point& source, const Point& impact, const Point& normal)
{
Point V = impact - source;
reflected = V - normal * 2.0f * (V|normal);
}
inline_ void DecomposeVector(Point& normal_compo, Point& tangent_compo, const Point& outward_dir, const Point& outward_normal)
{
normal_compo = outward_normal * (outward_dir|outward_normal);
tangent_compo = outward_dir - normal_compo;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Transforms a direction vector from world space to local space
* \param local_dir [out] direction vector in local space
* \param world_dir [in] direction vector in world space
* \param world [in] world transform
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ void ComputeLocalDirection(Point& local_dir, const Point& world_dir, const Matrix4x4& world)
{
// Get world direction back in local space
// Matrix3x3 InvWorld = world;
// local_dir = InvWorld * world_dir;
local_dir = Matrix3x3(world) * world_dir;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Transforms a position vector from world space to local space
* \param local_pt [out] position vector in local space
* \param world_pt [in] position vector in world space
* \param world [in] world transform
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ void ComputeLocalPoint(Point& local_pt, const Point& world_pt, const Matrix4x4& world)
{
// Get world vertex back in local space
Matrix4x4 InvWorld = world;
InvWorld.Invert();
local_pt = world_pt * InvWorld;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Transforms a ray from world space to local space
* \param local_ray [out] ray in local space
* \param world_ray [in] ray in world space
* \param world [in] world transform
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ void ComputeLocalRay(Ray& local_ray, const Ray& world_ray, const Matrix4x4& world)
{
// Get world ray back in local space
ComputeLocalDirection(local_ray.mDir, world_ray.mDir, world);
ComputeLocalPoint(local_ray.mOrig, world_ray.mOrig, world);
}
#endif // __ICERAY_H__

520
ode/OPCODE/Ice/IceRevisitedRadix.cpp Normal file
View File

@ -0,0 +1,520 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains source code from the article "Radix Sort Revisited".
* \file IceRevisitedRadix.cpp
* \author Pierre Terdiman
* \date April, 4, 2000
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Revisited Radix Sort.
* This is my new radix routine:
* - it uses indices and doesn't recopy the values anymore, hence wasting less ram
* - it creates all the histograms in one run instead of four
* - it sorts words faster than dwords and bytes faster than words
* - it correctly sorts negative floating-point values by patching the offsets
* - it automatically takes advantage of temporal coherence
* - multiple keys support is a side effect of temporal coherence
* - it may be worth recoding in asm... (mainly to use FCOMI, FCMOV, etc) [it's probably memory-bound anyway]
*
* History:
* - 08.15.98: very first version
* - 04.04.00: recoded for the radix article
* - 12.xx.00: code lifting
* - 09.18.01: faster CHECK_PASS_VALIDITY thanks to Mark D. Shattuck (who provided other tips, not included here)
* - 10.11.01: added local ram support
* - 01.20.02: bugfix! In very particular cases the last pass was skipped in the float code-path, leading to incorrect sorting......
* - 01.02.02: - "mIndices" renamed => "mRanks". That's a rank sorter after all.
* - ranks are not "reset" anymore, but implicit on first calls
* - 07.05.02: - offsets rewritten with one less indirection.
* - 11.03.02: - "bool" replaced with RadixHint enum
*
* \class RadixSort
* \author Pierre Terdiman
* \version 1.4
* \date August, 15, 1998
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
To do:
- add an offset parameter between two input values (avoid some data recopy sometimes)
- unroll ? asm ?
- 11 bits trick & 3 passes as Michael did
- prefetch stuff the day I have a P3
- make a version with 16-bits indices ?
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Precompiled Header
#include "Stdafx.h"
using namespace IceCore;
#define INVALIDATE_RANKS mCurrentSize|=0x80000000
#define VALIDATE_RANKS mCurrentSize&=0x7fffffff
#define CURRENT_SIZE (mCurrentSize&0x7fffffff)
#define INVALID_RANKS (mCurrentSize&0x80000000)
#define CHECK_RESIZE(n) \
if(n!=mPreviousSize) \
{ \
if(n>mCurrentSize) Resize(n); \
else ResetRanks(); \
mPreviousSize = n; \
}
#define CREATE_HISTOGRAMS(type, buffer) \
/* Clear counters/histograms */ \
ZeroMemory(mHistogram, 256*4*sizeof(udword)); \
\
/* Prepare to count */ \
ubyte* p = (ubyte*)input; \
ubyte* pe = &p[nb*4]; \
udword* h0= &mHistogram[0]; /* Histogram for first pass (LSB) */ \
udword* h1= &mHistogram[256]; /* Histogram for second pass */ \
udword* h2= &mHistogram[512]; /* Histogram for third pass */ \
udword* h3= &mHistogram[768]; /* Histogram for last pass (MSB) */ \
\
bool AlreadySorted = true; /* Optimism... */ \
\
if(INVALID_RANKS) \
{ \
/* Prepare for temporal coherence */ \
type* Running = (type*)buffer; \
type PrevVal = *Running; \
\
while(p!=pe) \
{ \
/* Read input buffer in previous sorted order */ \
type Val = *Running++; \
/* Check whether already sorted or not */ \
if(Val<PrevVal) { AlreadySorted = false; break; } /* Early out */ \
/* Update for next iteration */ \
PrevVal = Val; \
\
/* Create histograms */ \
h0[*p++]++; h1[*p++]++; h2[*p++]++; h3[*p++]++; \
} \
\
/* If all input values are already sorted, we just have to return and leave the */ \
/* previous list unchanged. That way the routine may take advantage of temporal */ \
/* coherence, for example when used to sort transparent faces. */ \
if(AlreadySorted) \
{ \
mNbHits++; \
for(udword i=0;i<nb;i++) mRanks[i] = i; \
return *this; \
} \
} \
else \
{ \
/* Prepare for temporal coherence */ \
udword* Indices = mRanks; \
type PrevVal = (type)buffer[*Indices]; \
\
while(p!=pe) \
{ \
/* Read input buffer in previous sorted order */ \
type Val = (type)buffer[*Indices++]; \
/* Check whether already sorted or not */ \
if(Val<PrevVal) { AlreadySorted = false; break; } /* Early out */ \
/* Update for next iteration */ \
PrevVal = Val; \
\
/* Create histograms */ \
h0[*p++]++; h1[*p++]++; h2[*p++]++; h3[*p++]++; \
} \
\
/* If all input values are already sorted, we just have to return and leave the */ \
/* previous list unchanged. That way the routine may take advantage of temporal */ \
/* coherence, for example when used to sort transparent faces. */ \
if(AlreadySorted) { mNbHits++; return *this; } \
} \
\
/* Else there has been an early out and we must finish computing the histograms */ \
while(p!=pe) \
{ \
/* Create histograms without the previous overhead */ \
h0[*p++]++; h1[*p++]++; h2[*p++]++; h3[*p++]++; \
}
#define CHECK_PASS_VALIDITY(pass) \
/* Shortcut to current counters */ \
udword* CurCount = &mHistogram[pass<<8]; \
\
/* Reset flag. The sorting pass is supposed to be performed. (default) */ \
bool PerformPass = true; \
\
/* Check pass validity */ \
\
/* If all values have the same byte, sorting is useless. */ \
/* It may happen when sorting bytes or words instead of dwords. */ \
/* This routine actually sorts words faster than dwords, and bytes */ \
/* faster than words. Standard running time (O(4*n))is reduced to O(2*n) */ \
/* for words and O(n) for bytes. Running time for floats depends on actual values... */ \
\
/* Get first byte */ \
ubyte UniqueVal = *(((ubyte*)input)+pass); \
\
/* Check that byte's counter */ \
if(CurCount[UniqueVal]==nb) PerformPass=false;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Constructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
RadixSort::RadixSort() : mRanks(null), mRanks2(null), mCurrentSize(0), mTotalCalls(0), mNbHits(0)
{
#ifndef RADIX_LOCAL_RAM
// Allocate input-independent ram
mHistogram = new udword[256*4];
mOffset = new udword[256];
#endif
// Initialize indices
INVALIDATE_RANKS;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Destructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
RadixSort::~RadixSort()
{
// Release everything
#ifndef RADIX_LOCAL_RAM
DELETEARRAY(mOffset);
DELETEARRAY(mHistogram);
#endif
DELETEARRAY(mRanks2);
DELETEARRAY(mRanks);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Resizes the inner lists.
* \param nb [in] new size (number of dwords)
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool RadixSort::Resize(udword nb)
{
// Free previously used ram
DELETEARRAY(mRanks2);
DELETEARRAY(mRanks);
// Get some fresh one
mRanks = new udword[nb]; CHECKALLOC(mRanks);
mRanks2 = new udword[nb]; CHECKALLOC(mRanks2);
return true;
}
inline_ void RadixSort::CheckResize(udword nb)
{
udword CurSize = CURRENT_SIZE;
if(nb!=CurSize)
{
if(nb>CurSize) Resize(nb);
mCurrentSize = nb;
INVALIDATE_RANKS;
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Main sort routine.
* This one is for integer values. After the call, mRanks contains a list of indices in sorted order, i.e. in the order you may process your data.
* \param input [in] a list of integer values to sort
* \param nb [in] number of values to sort, must be < 2^31
* \param hint [in] RADIX_SIGNED to handle negative values, RADIX_UNSIGNED if you know your input buffer only contains positive values
* \return Self-Reference
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
RadixSort& RadixSort::Sort(const udword* input, udword nb, RadixHint hint)
{
// Checkings
if(!input || !nb || nb&0x80000000) return *this;
// Stats
mTotalCalls++;
// Resize lists if needed
CheckResize(nb);
#ifdef RADIX_LOCAL_RAM
// Allocate histograms & offsets on the stack
udword mHistogram[256*4];
// udword mOffset[256];
udword* mLink[256];
#endif
// Create histograms (counters). Counters for all passes are created in one run.
// Pros: read input buffer once instead of four times
// Cons: mHistogram is 4Kb instead of 1Kb
// We must take care of signed/unsigned values for temporal coherence.... I just
// have 2 code paths even if just a single opcode changes. Self-modifying code, someone?
if(hint==RADIX_UNSIGNED) { CREATE_HISTOGRAMS(udword, input); }
else { CREATE_HISTOGRAMS(sdword, input); }
// Compute #negative values involved if needed
udword NbNegativeValues = 0;
if(hint==RADIX_SIGNED)
{
// An efficient way to compute the number of negatives values we'll have to deal with is simply to sum the 128
// last values of the last histogram. Last histogram because that's the one for the Most Significant Byte,
// responsible for the sign. 128 last values because the 128 first ones are related to positive numbers.
udword* h3= &mHistogram[768];
for(udword i=128;i<256;i++) NbNegativeValues += h3[i]; // 768 for last histogram, 128 for negative part
}
// Radix sort, j is the pass number (0=LSB, 3=MSB)
for(udword j=0;j<4;j++)
{
CHECK_PASS_VALIDITY(j);
// Sometimes the fourth (negative) pass is skipped because all numbers are negative and the MSB is 0xFF (for example). This is
// not a problem, numbers are correctly sorted anyway.
if(PerformPass)
{
// Should we care about negative values?
if(j!=3 || hint==RADIX_UNSIGNED)
{
// Here we deal with positive values only
// Create offsets
// mOffset[0] = 0;
// for(udword i=1;i<256;i++) mOffset[i] = mOffset[i-1] + CurCount[i-1];
mLink[0] = mRanks2;
for(udword i=1;i<256;i++) mLink[i] = mLink[i-1] + CurCount[i-1];
}
else
{
// This is a special case to correctly handle negative integers. They're sorted in the right order but at the wrong place.
// Create biased offsets, in order for negative numbers to be sorted as well
// mOffset[0] = NbNegativeValues; // First positive number takes place after the negative ones
mLink[0] = &mRanks2[NbNegativeValues]; // First positive number takes place after the negative ones
// for(udword i=1;i<128;i++) mOffset[i] = mOffset[i-1] + CurCount[i-1]; // 1 to 128 for positive numbers
for(udword i=1;i<128;i++) mLink[i] = mLink[i-1] + CurCount[i-1]; // 1 to 128 for positive numbers
// Fixing the wrong place for negative values
// mOffset[128] = 0;
mLink[128] = mRanks2;
// for(i=129;i<256;i++) mOffset[i] = mOffset[i-1] + CurCount[i-1];
for(udword i=129;i<256;i++) mLink[i] = mLink[i-1] + CurCount[i-1];
}
// Perform Radix Sort
ubyte* InputBytes = (ubyte*)input;
InputBytes += j;
if(INVALID_RANKS)
{
// for(udword i=0;i<nb;i++) mRanks2[mOffset[InputBytes[i<<2]]++] = i;
for(udword i=0;i<nb;i++) *mLink[InputBytes[i<<2]]++ = i;
VALIDATE_RANKS;
}
else
{
udword* Indices = mRanks;
udword* IndicesEnd = &mRanks[nb];
while(Indices!=IndicesEnd)
{
udword id = *Indices++;
// mRanks2[mOffset[InputBytes[id<<2]]++] = id;
*mLink[InputBytes[id<<2]]++ = id;
}
}
// Swap pointers for next pass. Valid indices - the most recent ones - are in mRanks after the swap.
udword* Tmp = mRanks; mRanks = mRanks2; mRanks2 = Tmp;
}
}
return *this;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Main sort routine.
* This one is for floating-point values. After the call, mRanks contains a list of indices in sorted order, i.e. in the order you may process your data.
* \param input [in] a list of floating-point values to sort
* \param nb [in] number of values to sort, must be < 2^31
* \return Self-Reference
* \warning only sorts IEEE floating-point values
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
RadixSort& RadixSort::Sort(const float* input2, udword nb)
{
// Checkings
if(!input2 || !nb || nb&0x80000000) return *this;
// Stats
mTotalCalls++;
udword* input = (udword*)input2;
// Resize lists if needed
CheckResize(nb);
#ifdef RADIX_LOCAL_RAM
// Allocate histograms & offsets on the stack
udword mHistogram[256*4];
// udword mOffset[256];
udword* mLink[256];
#endif
// Create histograms (counters). Counters for all passes are created in one run.
// Pros: read input buffer once instead of four times
// Cons: mHistogram is 4Kb instead of 1Kb
// Floating-point values are always supposed to be signed values, so there's only one code path there.
// Please note the floating point comparison needed for temporal coherence! Although the resulting asm code
// is dreadful, this is surprisingly not such a performance hit - well, I suppose that's a big one on first
// generation Pentiums....We can't make comparison on integer representations because, as Chris said, it just
// wouldn't work with mixed positive/negative values....
{ CREATE_HISTOGRAMS(float, input2); }
// Compute #negative values involved if needed
udword NbNegativeValues = 0;
// An efficient way to compute the number of negatives values we'll have to deal with is simply to sum the 128
// last values of the last histogram. Last histogram because that's the one for the Most Significant Byte,
// responsible for the sign. 128 last values because the 128 first ones are related to positive numbers.
udword* h3= &mHistogram[768];
for(udword i=128;i<256;i++) NbNegativeValues += h3[i]; // 768 for last histogram, 128 for negative part
// Radix sort, j is the pass number (0=LSB, 3=MSB)
for(udword j=0;j<4;j++)
{
// Should we care about negative values?
if(j!=3)
{
// Here we deal with positive values only
CHECK_PASS_VALIDITY(j);
if(PerformPass)
{
// Create offsets
// mOffset[0] = 0;
mLink[0] = mRanks2;
// for(udword i=1;i<256;i++) mOffset[i] = mOffset[i-1] + CurCount[i-1];
for(udword i=1;i<256;i++) mLink[i] = mLink[i-1] + CurCount[i-1];
// Perform Radix Sort
ubyte* InputBytes = (ubyte*)input;
InputBytes += j;
if(INVALID_RANKS)
{
// for(i=0;i<nb;i++) mRanks2[mOffset[InputBytes[i<<2]]++] = i;
for(udword i=0;i<nb;i++) *mLink[InputBytes[i<<2]]++ = i;
VALIDATE_RANKS;
}
else
{
udword* Indices = mRanks;
udword* IndicesEnd = &mRanks[nb];
while(Indices!=IndicesEnd)
{
udword id = *Indices++;
// mRanks2[mOffset[InputBytes[id<<2]]++] = id;
*mLink[InputBytes[id<<2]]++ = id;
}
}
// Swap pointers for next pass. Valid indices - the most recent ones - are in mRanks after the swap.
udword* Tmp = mRanks; mRanks = mRanks2; mRanks2 = Tmp;
}
}
else
{
// This is a special case to correctly handle negative values
CHECK_PASS_VALIDITY(j);
if(PerformPass)
{
// Create biased offsets, in order for negative numbers to be sorted as well
// mOffset[0] = NbNegativeValues; // First positive number takes place after the negative ones
mLink[0] = &mRanks2[NbNegativeValues]; // First positive number takes place after the negative ones
// for(udword i=1;i<128;i++) mOffset[i] = mOffset[i-1] + CurCount[i-1]; // 1 to 128 for positive numbers
for(udword i=1;i<128;i++) mLink[i] = mLink[i-1] + CurCount[i-1]; // 1 to 128 for positive numbers
// We must reverse the sorting order for negative numbers!
// mOffset[255] = 0;
mLink[255] = mRanks2;
// for(i=0;i<127;i++) mOffset[254-i] = mOffset[255-i] + CurCount[255-i]; // Fixing the wrong order for negative values
for(udword i=0;i<127;i++) mLink[254-i] = mLink[255-i] + CurCount[255-i]; // Fixing the wrong order for negative values
// for(i=128;i<256;i++) mOffset[i] += CurCount[i]; // Fixing the wrong place for negative values
for(udword i=128;i<256;i++) mLink[i] += CurCount[i]; // Fixing the wrong place for negative values
// Perform Radix Sort
if(INVALID_RANKS)
{
for(udword i=0;i<nb;i++)
{
udword Radix = input[i]>>24; // Radix byte, same as above. AND is useless here (udword).
// ### cmp to be killed. Not good. Later.
// if(Radix<128) mRanks2[mOffset[Radix]++] = i; // Number is positive, same as above
// else mRanks2[--mOffset[Radix]] = i; // Number is negative, flip the sorting order
if(Radix<128) *mLink[Radix]++ = i; // Number is positive, same as above
else *(--mLink[Radix]) = i; // Number is negative, flip the sorting order
}
VALIDATE_RANKS;
}
else
{
for(udword i=0;i<nb;i++)
{
udword Radix = input[mRanks[i]]>>24; // Radix byte, same as above. AND is useless here (udword).
// ### cmp to be killed. Not good. Later.
// if(Radix<128) mRanks2[mOffset[Radix]++] = mRanks[i]; // Number is positive, same as above
// else mRanks2[--mOffset[Radix]] = mRanks[i]; // Number is negative, flip the sorting order
if(Radix<128) *mLink[Radix]++ = mRanks[i]; // Number is positive, same as above
else *(--mLink[Radix]) = mRanks[i]; // Number is negative, flip the sorting order
}
}
// Swap pointers for next pass. Valid indices - the most recent ones - are in mRanks after the swap.
udword* Tmp = mRanks; mRanks = mRanks2; mRanks2 = Tmp;
}
else
{
// The pass is useless, yet we still have to reverse the order of current list if all values are negative.
if(UniqueVal>=128)
{
if(INVALID_RANKS)
{
// ###Possible?
for(udword i=0;i<nb;i++) mRanks2[i] = nb-i-1;
VALIDATE_RANKS;
}
else
{
for(udword i=0;i<nb;i++) mRanks2[i] = mRanks[nb-i-1];
}
// Swap pointers for next pass. Valid indices - the most recent ones - are in mRanks after the swap.
udword* Tmp = mRanks; mRanks = mRanks2; mRanks2 = Tmp;
}
}
}
}
return *this;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Gets the ram used.
* \return memory used in bytes
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
udword RadixSort::GetUsedRam() const
{
udword UsedRam = sizeof(RadixSort);
#ifndef RADIX_LOCAL_RAM
UsedRam += 256*4*sizeof(udword); // Histograms
UsedRam += 256*sizeof(udword); // Offsets
#endif
UsedRam += 2*CURRENT_SIZE*sizeof(udword); // 2 lists of indices
return UsedRam;
}

65
ode/OPCODE/Ice/IceRevisitedRadix.h Normal file
View File

@ -0,0 +1,65 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains source code from the article "Radix Sort Revisited".
* \file IceRevisitedRadix.h
* \author Pierre Terdiman
* \date April, 4, 2000
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __ICERADIXSORT_H__
#define __ICERADIXSORT_H__
//! Allocate histograms & offsets locally
#define RADIX_LOCAL_RAM
enum RadixHint
{
RADIX_SIGNED, //!< Input values are signed
RADIX_UNSIGNED, //!< Input values are unsigned
RADIX_FORCE_DWORD = 0x7fffffff
};
class ICECORE_API RadixSort
{
public:
// Constructor/Destructor
RadixSort();
~RadixSort();
// Sorting methods
RadixSort& Sort(const udword* input, udword nb, RadixHint hint=RADIX_SIGNED);
RadixSort& Sort(const float* input, udword nb);
//! Access to results. mRanks is a list of indices in sorted order, i.e. in the order you may further process your data
inline_ const udword* GetRanks() const { return mRanks; }
//! mIndices2 gets trashed on calling the sort routine, but otherwise you can recycle it the way you want.
inline_ udword* GetRecyclable() const { return mRanks2; }
// Stats
udword GetUsedRam() const;
//! Returns the total number of calls to the radix sorter.
inline_ udword GetNbTotalCalls() const { return mTotalCalls; }
//! Returns the number of eraly exits due to temporal coherence.
inline_ udword GetNbHits() const { return mNbHits; }
private:
#ifndef RADIX_LOCAL_RAM
udword* mHistogram; //!< Counters for each byte
udword* mOffset; //!< Offsets (nearly a cumulative distribution function)
#endif
udword mCurrentSize; //!< Current size of the indices list
udword* mRanks; //!< Two lists, swapped each pass
udword* mRanks2;
// Stats
udword mTotalCalls; //!< Total number of calls to the sort routine
udword mNbHits; //!< Number of early exits due to coherence
// Internal methods
void CheckResize(udword nb);
bool Resize(udword nb);
};
#endif // __ICERADIXSORT_H__

57
ode/OPCODE/Ice/IceSegment.cpp Normal file
View File

@ -0,0 +1,57 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for segments.
* \file IceSegment.cpp
* \author Pierre Terdiman
* \date April, 4, 2000
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Segment class.
* A segment is defined by S(t) = mP0 * (1 - t) + mP1 * t, with 0 <= t <= 1
* Alternatively, a segment is S(t) = Origin + t * Direction for 0 <= t <= 1.
* Direction is not necessarily unit length. The end points are Origin = mP0 and Origin + Direction = mP1.
*
* \class Segment
* \author Pierre Terdiman
* \version 1.0
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Precompiled Header
#include "Stdafx.h"
using namespace IceMaths;
float Segment::SquareDistance(const Point& point, float* t) const
{
Point Diff = point - mP0;
Point Dir = mP1 - mP0;
float fT = Diff | Dir;
if(fT<=0.0f)
{
fT = 0.0f;
}
else
{
float SqrLen= Dir.SquareMagnitude();
if(fT>=SqrLen)
{
fT = 1.0f;
Diff -= Dir;
}
else
{
fT /= SqrLen;
Diff -= fT*Dir;
}
}
if(t) *t = fT;
return Diff.SquareMagnitude();
}

55
ode/OPCODE/Ice/IceSegment.h Normal file
View File

@ -0,0 +1,55 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for segments.
* \file IceSegment.h
* \author Pierre Terdiman
* \date April, 4, 2000
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __ICESEGMENT_H__
#define __ICESEGMENT_H__
class ICEMATHS_API Segment
{
public:
//! Constructor
inline_ Segment() {}
//! Constructor
inline_ Segment(const Point& p0, const Point& p1) : mP0(p0), mP1(p1) {}
//! Copy constructor
inline_ Segment(const Segment& seg) : mP0(seg.mP0), mP1(seg.mP1) {}
//! Destructor
inline_ ~Segment() {}
inline_ const Point& GetOrigin() const { return mP0; }
inline_ Point ComputeDirection() const { return mP1 - mP0; }
inline_ void ComputeDirection(Point& dir) const { dir = mP1 - mP0; }
inline_ float ComputeLength() const { return mP1.Distance(mP0); }
inline_ float ComputeSquareLength() const { return mP1.SquareDistance(mP0); }
inline_ void SetOriginDirection(const Point& origin, const Point& direction)
{
mP0 = mP1 = origin;
mP1 += direction;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes a point on the segment
* \param pt [out] point on segment
* \param t [in] point's parameter [t=0 => pt = mP0, t=1 => pt = mP1]
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ void ComputePoint(Point& pt, float t) const { pt = mP0 + t * (mP1 - mP0); }
float SquareDistance(const Point& point, float* t=null) const;
inline_ float Distance(const Point& point, float* t=null) const { return sqrtf(SquareDistance(point, t)); }
Point mP0; //!< Start of segment
Point mP1; //!< End of segment
};
#endif // __ICESEGMENT_H__

61
ode/OPCODE/Ice/IceTriList.h Normal file
View File

@ -0,0 +1,61 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for a triangle container.
* \file IceTrilist.h
* \author Pierre Terdiman
* \date April, 4, 2000
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __ICETRILIST_H__
#define __ICETRILIST_H__
class ICEMATHS_API TriList : public Container
{
public:
// Constructor / Destructor
TriList() {}
~TriList() {}
inline_ udword GetNbTriangles() const { return GetNbEntries()/9; }
inline_ Triangle* GetTriangles() const { return (Triangle*)GetEntries(); }
void AddTri(const Triangle& tri)
{
Add(tri.mVerts[0].x).Add(tri.mVerts[0].y).Add(tri.mVerts[0].z);
Add(tri.mVerts[1].x).Add(tri.mVerts[1].y).Add(tri.mVerts[1].z);
Add(tri.mVerts[2].x).Add(tri.mVerts[2].y).Add(tri.mVerts[2].z);
}
void AddTri(const Point& p0, const Point& p1, const Point& p2)
{
Add(p0.x).Add(p0.y).Add(p0.z);
Add(p1.x).Add(p1.y).Add(p1.z);
Add(p2.x).Add(p2.y).Add(p2.z);
}
};
class ICEMATHS_API TriangleList : public Container
{
public:
// Constructor / Destructor
TriangleList() {}
~TriangleList() {}
inline_ udword GetNbTriangles() const { return GetNbEntries()/3; }
inline_ IndexedTriangle* GetTriangles() const { return (IndexedTriangle*)GetEntries();}
void AddTriangle(const IndexedTriangle& tri)
{
Add(tri.mVRef[0]).Add(tri.mVRef[1]).Add(tri.mVRef[2]);
}
void AddTriangle(udword vref0, udword vref1, udword vref2)
{
Add(vref0).Add(vref1).Add(vref2);
}
};
#endif //__ICETRILIST_H__

286
ode/OPCODE/Ice/IceTriangle.cpp Normal file
View File

@ -0,0 +1,286 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains a handy triangle class.
* \file IceTriangle.cpp
* \author Pierre Terdiman
* \date January, 17, 2000
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Precompiled Header
#include "Stdafx.h"
using namespace IceMaths;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains a triangle class.
*
* \class Tri
* \author Pierre Terdiman
* \version 1.0
* \date 08.15.98
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
static sdword VPlaneSideEps(const Point& v, const Plane& plane, float epsilon)
{
// Compute distance from current vertex to the plane
float Dist = plane.Distance(v);
// Compute side:
// 1 = the vertex is on the positive side of the plane
// -1 = the vertex is on the negative side of the plane
// 0 = the vertex is on the plane (within epsilon)
return Dist > epsilon ? 1 : Dist < -epsilon ? -1 : 0;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Flips the winding order.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void Triangle::Flip()
{
Point Tmp = mVerts[1];
mVerts[1] = mVerts[2];
mVerts[2] = Tmp;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the triangle area.
* \return the area
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
float Triangle::Area() const
{
const Point& p0 = mVerts[0];
const Point& p1 = mVerts[1];
const Point& p2 = mVerts[2];
return ((p0 - p1)^(p0 - p2)).Magnitude() * 0.5f;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the triangle perimeter.
* \return the perimeter
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
float Triangle::Perimeter() const
{
const Point& p0 = mVerts[0];
const Point& p1 = mVerts[1];
const Point& p2 = mVerts[2];
return p0.Distance(p1)
+ p0.Distance(p2)
+ p1.Distance(p2);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the triangle compacity.
* \return the compacity
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
float Triangle::Compacity() const
{
float P = Perimeter();
if(P==0.0f) return 0.0f;
return (4.0f*PI*Area()/(P*P));
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the triangle normal.
* \param normal [out] the computed normal
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void Triangle::Normal(Point& normal) const
{
const Point& p0 = mVerts[0];
const Point& p1 = mVerts[1];
const Point& p2 = mVerts[2];
normal = ((p0 - p1)^(p0 - p2)).Normalize();
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the triangle denormalized normal.
* \param normal [out] the computed normal
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void Triangle::DenormalizedNormal(Point& normal) const
{
const Point& p0 = mVerts[0];
const Point& p1 = mVerts[1];
const Point& p2 = mVerts[2];
normal = ((p0 - p1)^(p0 - p2));
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the triangle center.
* \param center [out] the computed center
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void Triangle::Center(Point& center) const
{
const Point& p0 = mVerts[0];
const Point& p1 = mVerts[1];
const Point& p2 = mVerts[2];
center = (p0 + p1 + p2)*INV3;
}
PartVal Triangle::TestAgainstPlane(const Plane& plane, float epsilon) const
{
bool Pos = false, Neg = false;
// Loop through all vertices
for(udword i=0;i<3;i++)
{
// Compute side:
sdword Side = VPlaneSideEps(mVerts[i], plane, epsilon);
if (Side < 0) Neg = true;
else if (Side > 0) Pos = true;
}
if (!Pos && !Neg) return TRI_ON_PLANE;
else if (Pos && Neg) return TRI_INTERSECT;
else if (Pos && !Neg) return TRI_PLUS_SPACE;
else if (!Pos && Neg) return TRI_MINUS_SPACE;
// What?!
return TRI_FORCEDWORD;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the triangle moment.
* \param m [out] the moment
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
void Triangle::ComputeMoment(Moment& m)
{
// Compute the area of the triangle
m.mArea = Area();
// Compute the centroid
Center(m.mCentroid);
// Second-order components. Handle zero-area faces.
Point& p = mVerts[0];
Point& q = mVerts[1];
Point& r = mVerts[2];
if(m.mArea==0.0f)
{
// This triangle has zero area. The second order components would be eliminated with the usual formula, so, for the
// sake of robustness we use an alternative form. These are the centroid and second-order components of the triangle's vertices.
m.mCovariance.m[0][0] = (p.x*p.x + q.x*q.x + r.x*r.x);
m.mCovariance.m[0][1] = (p.x*p.y + q.x*q.y + r.x*r.y);
m.mCovariance.m[0][2] = (p.x*p.z + q.x*q.z + r.x*r.z);
m.mCovariance.m[1][1] = (p.y*p.y + q.y*q.y + r.y*r.y);
m.mCovariance.m[1][2] = (p.y*p.z + q.y*q.z + r.y*r.z);
m.mCovariance.m[2][2] = (p.z*p.z + q.z*q.z + r.z*r.z);
m.mCovariance.m[2][1] = m.mCovariance.m[1][2];
m.mCovariance.m[1][0] = m.mCovariance.m[0][1];
m.mCovariance.m[2][0] = m.mCovariance.m[0][2];
}
else
{
const float OneOverTwelve = 1.0f / 12.0f;
m.mCovariance.m[0][0] = m.mArea * (9.0f * m.mCentroid.x*m.mCentroid.x + p.x*p.x + q.x*q.x + r.x*r.x) * OneOverTwelve;
m.mCovariance.m[0][1] = m.mArea * (9.0f * m.mCentroid.x*m.mCentroid.y + p.x*p.y + q.x*q.y + r.x*r.y) * OneOverTwelve;
m.mCovariance.m[1][1] = m.mArea * (9.0f * m.mCentroid.y*m.mCentroid.y + p.y*p.y + q.y*q.y + r.y*r.y) * OneOverTwelve;
m.mCovariance.m[0][2] = m.mArea * (9.0f * m.mCentroid.x*m.mCentroid.z + p.x*p.z + q.x*q.z + r.x*r.z) * OneOverTwelve;
m.mCovariance.m[1][2] = m.mArea * (9.0f * m.mCentroid.y*m.mCentroid.z + p.y*p.z + q.y*q.z + r.y*r.z) * OneOverTwelve;
m.mCovariance.m[2][2] = m.mArea * (9.0f * m.mCentroid.z*m.mCentroid.z + p.z*p.z + q.z*q.z + r.z*r.z) * OneOverTwelve;
m.mCovariance.m[2][1] = m.mCovariance.m[1][2];
m.mCovariance.m[1][0] = m.mCovariance.m[0][1];
m.mCovariance.m[2][0] = m.mCovariance.m[0][2];
}
}
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the triangle's smallest edge length.
* \return the smallest edge length
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
float Triangle::MinEdgeLength() const
{
float Min = MAX_FLOAT;
float Length01 = mVerts[0].Distance(mVerts[1]);
float Length02 = mVerts[0].Distance(mVerts[2]);
float Length12 = mVerts[1].Distance(mVerts[2]);
if(Length01 < Min) Min = Length01;
if(Length02 < Min) Min = Length02;
if(Length12 < Min) Min = Length12;
return Min;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the triangle's largest edge length.
* \return the largest edge length
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
float Triangle::MaxEdgeLength() const
{
float Max = MIN_FLOAT;
float Length01 = mVerts[0].Distance(mVerts[1]);
float Length02 = mVerts[0].Distance(mVerts[2]);
float Length12 = mVerts[1].Distance(mVerts[2]);
if(Length01 > Max) Max = Length01;
if(Length02 > Max) Max = Length02;
if(Length12 > Max) Max = Length12;
return Max;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes a point on the triangle according to the stabbing information.
* \param u,v [in] point's barycentric coordinates
* \param pt [out] point on triangle
* \param nearvtx [out] index of nearest vertex
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void Triangle::ComputePoint(float u, float v, Point& pt, udword* nearvtx) const
{
// Compute point coordinates
pt = (1.0f - u - v)*mVerts[0] + u*mVerts[1] + v*mVerts[2];
// Compute nearest vertex if needed
if(nearvtx)
{
// Compute distance vector
Point d(mVerts[0].SquareDistance(pt), // Distance^2 from vertex 0 to point on the face
mVerts[1].SquareDistance(pt), // Distance^2 from vertex 1 to point on the face
mVerts[2].SquareDistance(pt)); // Distance^2 from vertex 2 to point on the face
// Get smallest distance
*nearvtx = d.SmallestAxis();
}
}
void Triangle::Inflate(float fat_coeff, bool constant_border)
{
// Compute triangle center
Point TriangleCenter;
Center(TriangleCenter);
// Don't normalize?
// Normalize => add a constant border, regardless of triangle size
// Don't => add more to big triangles
for(udword i=0;i<3;i++)
{
Point v = mVerts[i] - TriangleCenter;
if(constant_border) v.Normalize();
mVerts[i] += v * fat_coeff;
}
}

68
ode/OPCODE/Ice/IceTriangle.h Normal file
View File

@ -0,0 +1,68 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains a handy triangle class.
* \file IceTriangle.h
* \author Pierre Terdiman
* \date January, 17, 2000
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __ICETRIANGLE_H__
#define __ICETRIANGLE_H__
// Forward declarations
class Moment;
// Partitioning values
enum PartVal
{
TRI_MINUS_SPACE = 0, //!< Triangle is in the negative space
TRI_PLUS_SPACE = 1, //!< Triangle is in the positive space
TRI_INTERSECT = 2, //!< Triangle intersects plane
TRI_ON_PLANE = 3, //!< Triangle and plane are coplanar
TRI_FORCEDWORD = 0x7fffffff
};
// A triangle class.
class ICEMATHS_API Triangle
{
public:
//! Constructor
inline_ Triangle() {}
//! Constructor
inline_ Triangle(const Point& p0, const Point& p1, const Point& p2) { mVerts[0]=p0; mVerts[1]=p1; mVerts[2]=p2; }
//! Copy constructor
inline_ Triangle(const Triangle& triangle)
{
mVerts[0] = triangle.mVerts[0];
mVerts[1] = triangle.mVerts[1];
mVerts[2] = triangle.mVerts[2];
}
//! Destructor
inline_ ~Triangle() {}
//! Vertices
Point mVerts[3];
// Methods
void Flip();
float Area() const;
float Perimeter() const;
float Compacity() const;
void Normal(Point& normal) const;
void DenormalizedNormal(Point& normal) const;
void Center(Point& center) const;
inline_ Plane PlaneEquation() const { return Plane(mVerts[0], mVerts[1], mVerts[2]); }
PartVal TestAgainstPlane(const Plane& plane, float epsilon) const;
// float Distance(Point& cp, Point& cq, Tri& tri);
void ComputeMoment(Moment& m);
float MinEdgeLength() const;
float MaxEdgeLength() const;
void ComputePoint(float u, float v, Point& pt, udword* nearvtx=null) const;
void Inflate(float fat_coeff, bool constant_border);
};
#endif // __ICETRIANGLE_H__

171
ode/OPCODE/Ice/IceTypes.h Normal file
View File

@ -0,0 +1,171 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains custom types.
* \file IceTypes.h
* \author Pierre Terdiman
* \date April, 4, 2000
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __ICETYPES_H__
#define __ICETYPES_H__
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Things to help us compile on non-windows platforms
#if defined(__MACOSX__) || defined(__APPLE__)
#undef bool
#define bool char
#undef true
#define true ((bool)-1)
#undef false
#define false ((bool)0)
#endif // mac stuff
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
#define USE_HANDLE_MANAGER
// Constants
#define PI 3.1415926535897932384626433832795028841971693993751f //!< PI
#define HALFPI 1.57079632679489661923f //!< 0.5 * PI
#define TWOPI 6.28318530717958647692f //!< 2.0 * PI
#define INVPI 0.31830988618379067154f //!< 1.0 / PI
#define RADTODEG 57.2957795130823208768f //!< 180.0 / PI, convert radians to degrees
#define DEGTORAD 0.01745329251994329577f //!< PI / 180.0, convert degrees to radians
#define EXP 2.71828182845904523536f //!< e
#define INVLOG2 3.32192809488736234787f //!< 1.0 / log10(2)
#define LN2 0.693147180559945f //!< ln(2)
#define INVLN2 1.44269504089f //!< 1.0f / ln(2)
#define INV3 0.33333333333333333333f //!< 1/3
#define INV6 0.16666666666666666666f //!< 1/6
#define INV7 0.14285714285714285714f //!< 1/7
#define INV9 0.11111111111111111111f //!< 1/9
#define INV255 0.00392156862745098039f //!< 1/255
#define SQRT2 1.41421356237f //!< sqrt(2)
#define INVSQRT2 0.707106781188f //!< 1 / sqrt(2)
#define SQRT3 1.73205080757f //!< sqrt(3)
#define INVSQRT3 0.577350269189f //!< 1 / sqrt(3)
#define null 0 //!< our own NULL pointer
// Custom types used in ICE
typedef signed char sbyte; //!< sizeof(sbyte) must be 1
typedef unsigned char ubyte; //!< sizeof(ubyte) must be 1
typedef signed short sword; //!< sizeof(sword) must be 2
typedef unsigned short uword; //!< sizeof(uword) must be 2
typedef signed int sdword; //!< sizeof(sdword) must be 4
typedef unsigned int udword; //!< sizeof(udword) must be 4
typedef signed __int64 sqword; //!< sizeof(sqword) must be 8
typedef unsigned __int64 uqword; //!< sizeof(uqword) must be 8
typedef float float32; //!< sizeof(float32) must be 4
typedef double float64; //!< sizeof(float64) must be 4
ICE_COMPILE_TIME_ASSERT(sizeof(bool)==1); // ...otherwise things might fail with VC++ 4.2 !
ICE_COMPILE_TIME_ASSERT(sizeof(ubyte)==1);
ICE_COMPILE_TIME_ASSERT(sizeof(sbyte)==1);
ICE_COMPILE_TIME_ASSERT(sizeof(sword)==2);
ICE_COMPILE_TIME_ASSERT(sizeof(uword)==2);
ICE_COMPILE_TIME_ASSERT(sizeof(udword)==4);
ICE_COMPILE_TIME_ASSERT(sizeof(sdword)==4);
ICE_COMPILE_TIME_ASSERT(sizeof(uqword)==8);
ICE_COMPILE_TIME_ASSERT(sizeof(sqword)==8);
//! TO BE DOCUMENTED
#define DECLARE_ICE_HANDLE(name) struct name##__ { int unused; }; typedef struct name##__ *name
typedef udword DynID; //!< Dynamic identifier
#ifdef USE_HANDLE_MANAGER
typedef udword KID; //!< Kernel ID
// DECLARE_ICE_HANDLE(KID);
#else
typedef uword KID; //!< Kernel ID
#endif
typedef udword RTYPE; //!< Relationship-type (!) between owners and references
#define INVALID_ID 0xffffffff //!< Invalid dword ID (counterpart of null pointers)
#ifdef USE_HANDLE_MANAGER
#define INVALID_KID 0xffffffff //!< Invalid Kernel ID
#else
#define INVALID_KID 0xffff //!< Invalid Kernel ID
#endif
#define INVALID_NUMBER 0xDEADBEEF //!< Standard junk value
// Define BOOL if needed
#ifndef BOOL
typedef int BOOL; //!< Another boolean type.
#endif
//! Union of a float and a sdword
typedef union {
float f; //!< The float
sdword d; //!< The integer
}scell;
//! Union of a float and a udword
typedef union {
float f; //!< The float
udword d; //!< The integer
}ucell;
// Type ranges
#define MAX_SBYTE 0x7f //!< max possible sbyte value
#define MIN_SBYTE 0x80 //!< min possible sbyte value
#define MAX_UBYTE 0xff //!< max possible ubyte value
#define MIN_UBYTE 0x00 //!< min possible ubyte value
#define MAX_SWORD 0x7fff //!< max possible sword value
#define MIN_SWORD 0x8000 //!< min possible sword value
#define MAX_UWORD 0xffff //!< max possible uword value
#define MIN_UWORD 0x0000 //!< min possible uword value
#define MAX_SDWORD 0x7fffffff //!< max possible sdword value
#define MIN_SDWORD 0x80000000 //!< min possible sdword value
#define MAX_UDWORD 0xffffffff //!< max possible udword value
#define MIN_UDWORD 0x00000000 //!< min possible udword value
#define MAX_FLOAT FLT_MAX //!< max possible float value
#define MIN_FLOAT (-FLT_MAX) //!< min possible loat value
#define IEEE_1_0 0x3f800000 //!< integer representation of 1.0
#define IEEE_255_0 0x437f0000 //!< integer representation of 255.0
#define IEEE_MAX_FLOAT 0x7f7fffff //!< integer representation of MAX_FLOAT
#define IEEE_MIN_FLOAT 0xff7fffff //!< integer representation of MIN_FLOAT
#define IEEE_UNDERFLOW_LIMIT 0x1a000000
#define ONE_OVER_RAND_MAX (1.0f / float(RAND_MAX)) //!< Inverse of the max possible value returned by rand()
typedef int (__stdcall* PROC)(); //!< A standard procedure call.
typedef bool (*ENUMERATION)(udword value, udword param, udword context); //!< ICE standard enumeration call
typedef void** VTABLE; //!< A V-Table.
#undef MIN
#undef MAX
#define MIN(a, b) ((a) < (b) ? (a) : (b)) //!< Returns the min value between a and b
#define MAX(a, b) ((a) > (b) ? (a) : (b)) //!< Returns the max value between a and b
#define MAXMAX(a,b,c) ((a) > (b) ? MAX (a,c) : MAX (b,c)) //!< Returns the max value between a, b and c
template<class T> inline_ const T& TMin (const T& a, const T& b) { return b < a ? b : a; }
template<class T> inline_ const T& TMax (const T& a, const T& b) { return a < b ? b : a; }
template<class T> inline_ void TSetMin (T& a, const T& b) { if(a>b) a = b; }
template<class T> inline_ void TSetMax (T& a, const T& b) { if(a<b) a = b; }
#define SQR(x) ((x)*(x)) //!< Returns x square
#define CUBE(x) ((x)*(x)*(x)) //!< Returns x cube
#define AND & //!< ...
#define OR | //!< ...
#define XOR ^ //!< ...
#define QUADRAT(x) ((x)*(x)) //!< Returns x square
#ifdef _WIN32
# define srand48(x) srand((unsigned int) (x))
# define srandom(x) srand((unsigned int) (x))
# define random() ((double) rand())
# define drand48() ((double) (((double) rand()) / ((double) RAND_MAX)))
#endif
#endif // __ICETYPES_H__

39
ode/OPCODE/Ice/IceUtils.cpp Normal file
View File

@ -0,0 +1,39 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains misc. useful macros & defines.
* \file IceUtils.cpp
* \author Pierre Terdiman (collected from various sources)
* \date April, 4, 2000
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Precompiled Header
#include "Stdafx.h"
using namespace IceCore;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Returns the alignment of the input address.
* \fn Alignment()
* \param address [in] address to check
* \return the best alignment (e.g. 1 for odd addresses, etc)
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
udword IceCore::Alignment(udword address)
{
// Returns 0 for null addresses
if(!address) return 0;
// Test all bits
udword Align = 1;
for(udword i=1;i<32;i++)
{
// Returns as soon as the alignment is broken
if(address&Align) return Align;
Align<<=1;
}
// Here all bits are null, except the highest one (else the address would be null)
return Align;
}

256
ode/OPCODE/Ice/IceUtils.h Normal file
View File

@ -0,0 +1,256 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains misc. useful macros & defines.
* \file IceUtils.h
* \author Pierre Terdiman (collected from various sources)
* \date April, 4, 2000
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __ICEUTILS_H__
#define __ICEUTILS_H__
#define START_RUNONCE { static bool __RunOnce__ = false; if(!__RunOnce__){
#define END_RUNONCE __RunOnce__ = true;}}
//! Reverse all the bits in a 32 bit word (from Steve Baker's Cute Code Collection)
//! (each line can be done in any order.
inline_ void ReverseBits(udword& n)
{
n = ((n >> 1) & 0x55555555) | ((n << 1) & 0xaaaaaaaa);
n = ((n >> 2) & 0x33333333) | ((n << 2) & 0xcccccccc);
n = ((n >> 4) & 0x0f0f0f0f) | ((n << 4) & 0xf0f0f0f0);
n = ((n >> 8) & 0x00ff00ff) | ((n << 8) & 0xff00ff00);
n = ((n >> 16) & 0x0000ffff) | ((n << 16) & 0xffff0000);
// Etc for larger intergers (64 bits in Java)
// NOTE: the >> operation must be unsigned! (>>> in java)
}
//! Count the number of '1' bits in a 32 bit word (from Steve Baker's Cute Code Collection)
inline_ udword CountBits(udword n)
{
// This relies of the fact that the count of n bits can NOT overflow
// an n bit interger. EG: 1 bit count takes a 1 bit interger, 2 bit counts
// 2 bit interger, 3 bit count requires only a 2 bit interger.
// So we add all bit pairs, then each nible, then each byte etc...
n = (n & 0x55555555) + ((n & 0xaaaaaaaa) >> 1);
n = (n & 0x33333333) + ((n & 0xcccccccc) >> 2);
n = (n & 0x0f0f0f0f) + ((n & 0xf0f0f0f0) >> 4);
n = (n & 0x00ff00ff) + ((n & 0xff00ff00) >> 8);
n = (n & 0x0000ffff) + ((n & 0xffff0000) >> 16);
// Etc for larger intergers (64 bits in Java)
// NOTE: the >> operation must be unsigned! (>>> in java)
return n;
}
//! Even faster?
inline_ udword CountBits2(udword bits)
{
bits = bits - ((bits >> 1) & 0x55555555);
bits = ((bits >> 2) & 0x33333333) + (bits & 0x33333333);
bits = ((bits >> 4) + bits) & 0x0F0F0F0F;
return (bits * 0x01010101) >> 24;
}
//! Spread out bits. EG 00001111 -> 0101010101
//! 00001010 -> 0100010000
//! This is used to interleve to intergers to produce a `Morten Key'
//! used in Space Filling Curves (See DrDobbs Journal, July 1999)
//! Order is important.
inline_ void SpreadBits(udword& n)
{
n = ( n & 0x0000ffff) | (( n & 0xffff0000) << 16);
n = ( n & 0x000000ff) | (( n & 0x0000ff00) << 8);
n = ( n & 0x000f000f) | (( n & 0x00f000f0) << 4);
n = ( n & 0x03030303) | (( n & 0x0c0c0c0c) << 2);
n = ( n & 0x11111111) | (( n & 0x22222222) << 1);
}
// Next Largest Power of 2
// Given a binary integer value x, the next largest power of 2 can be computed by a SWAR algorithm
// that recursively "folds" the upper bits into the lower bits. This process yields a bit vector with
// the same most significant 1 as x, but all 1's below it. Adding 1 to that value yields the next
// largest power of 2. For a 32-bit value:
inline_ udword nlpo2(udword x)
{
x |= (x >> 1);
x |= (x >> 2);
x |= (x >> 4);
x |= (x >> 8);
x |= (x >> 16);
return x+1;
}
//! Test to see if a number is an exact power of two (from Steve Baker's Cute Code Collection)
inline_ bool IsPowerOfTwo(udword n) { return ((n&(n-1))==0); }
//! Zero the least significant '1' bit in a word. (from Steve Baker's Cute Code Collection)
inline_ void ZeroLeastSetBit(udword& n) { n&=(n-1); }
//! Set the least significant N bits in a word. (from Steve Baker's Cute Code Collection)
inline_ void SetLeastNBits(udword& x, udword n) { x|=~(~0<<n); }
//! Classic XOR swap (from Steve Baker's Cute Code Collection)
//! x ^= y; /* x' = (x^y) */
//! y ^= x; /* y' = (y^(x^y)) = x */
//! x ^= y; /* x' = (x^y)^x = y */
inline_ void Swap(udword& x, udword& y) { x ^= y; y ^= x; x ^= y; }
//! Little/Big endian (from Steve Baker's Cute Code Collection)
//!
//! Extra comments by Kenny Hoff:
//! Determines the byte-ordering of the current machine (little or big endian)
//! by setting an integer value to 1 (so least significant bit is now 1); take
//! the address of the int and cast to a byte pointer (treat integer as an
//! array of four bytes); check the value of the first byte (must be 0 or 1).
//! If the value is 1, then the first byte least significant byte and this
//! implies LITTLE endian. If the value is 0, the first byte is the most
//! significant byte, BIG endian. Examples:
//! integer 1 on BIG endian: 00000000 00000000 00000000 00000001
//! integer 1 on LITTLE endian: 00000001 00000000 00000000 00000000
//!---------------------------------------------------------------------------
//! int IsLittleEndian() { int x=1; return ( ((char*)(&x))[0] ); }
inline_ char LittleEndian() { int i = 1; return *((char*)&i); }
//!< Alternative abs function
inline_ udword abs_(sdword x) { sdword y= x >> 31; return (x^y)-y; }
//!< Alternative min function
inline_ sdword min_(sdword a, sdword b) { sdword delta = b-a; return a + (delta&(delta>>31)); }
// Determine if one of the bytes in a 4 byte word is zero
inline_ BOOL HasNullByte(udword x) { return ((x + 0xfefefeff) & (~x) & 0x80808080); }
// To find the smallest 1 bit in a word EG: ~~~~~~10---0 => 0----010---0
inline_ udword LowestOneBit(udword w) { return ((w) & (~(w)+1)); }
// inline_ udword LowestOneBit_(udword w) { return ((w) & (-(w))); }
// Most Significant 1 Bit
// Given a binary integer value x, the most significant 1 bit (highest numbered element of a bit set)
// can be computed using a SWAR algorithm that recursively "folds" the upper bits into the lower bits.
// This process yields a bit vector with the same most significant 1 as x, but all 1's below it.
// Bitwise AND of the original value with the complement of the "folded" value shifted down by one
// yields the most significant bit. For a 32-bit value:
inline_ udword msb32(udword x)
{
x |= (x >> 1);
x |= (x >> 2);
x |= (x >> 4);
x |= (x >> 8);
x |= (x >> 16);
return (x & ~(x >> 1));
}
/*
"Just call it repeatedly with various input values and always with the same variable as "memory".
The sharpness determines the degree of filtering, where 0 completely filters out the input, and 1
does no filtering at all.
I seem to recall from college that this is called an IIR (Infinite Impulse Response) filter. As opposed
to the more typical FIR (Finite Impulse Response).
Also, I'd say that you can make more intelligent and interesting filters than this, for example filters
that remove wrong responses from the mouse because it's being moved too fast. You'd want such a filter
to be applied before this one, of course."
(JCAB on Flipcode)
*/
inline_ float FeedbackFilter(float val, float& memory, float sharpness)
{
ASSERT(sharpness>=0.0f && sharpness<=1.0f && "Invalid sharpness value in feedback filter");
if(sharpness<0.0f) sharpness = 0.0f;
else if(sharpness>1.0f) sharpness = 1.0f;
return memory = val * sharpness + memory * (1.0f - sharpness);
}
//! If you can guarantee that your input domain (i.e. value of x) is slightly
//! limited (abs(x) must be < ((1<<31u)-32767)), then you can use the
//! following code to clamp the resulting value into [-32768,+32767] range:
inline_ int ClampToInt16(int x)
{
// ASSERT(abs(x) < (int)((1<<31u)-32767));
int delta = 32767 - x;
x += (delta>>31) & delta;
delta = x + 32768;
x -= (delta>>31) & delta;
return x;
}
// Generic functions
template<class Type> inline_ void TSwap(Type& a, Type& b) { const Type c = a; a = b; b = c; }
template<class Type> inline_ Type TClamp(const Type& x, const Type& lo, const Type& hi) { return ((x<lo) ? lo : (x>hi) ? hi : x); }
template<class Type> inline_ void TSort(Type& a, Type& b)
{
if(a>b) TSwap(a, b);
}
template<class Type> inline_ void TSort(Type& a, Type& b, Type& c)
{
if(a>b) TSwap(a, b);
if(b>c) TSwap(b, c);
if(a>b) TSwap(a, b);
if(b>c) TSwap(b, c);
}
// Prevent nasty user-manipulations (strategy borrowed from Charles Bloom)
// #define PREVENT_COPY(curclass) void operator = (const curclass& object) { ASSERT(!"Bad use of operator ="); }
// ... actually this is better !
#define PREVENT_COPY(cur_class) private: cur_class(const cur_class& object); cur_class& operator=(const cur_class& object);
//! TO BE DOCUMENTED
#define OFFSET_OF(Class, Member) (size_t)&(((Class*)0)->Member)
//! TO BE DOCUMENTED
#define ARRAYSIZE(p) (sizeof(p)/sizeof(p[0]))
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Returns the alignment of the input address.
* \fn Alignment()
* \param address [in] address to check
* \return the best alignment (e.g. 1 for odd addresses, etc)
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
FUNCTION ICECORE_API udword Alignment(udword address);
#define IS_ALIGNED_2(x) ((x&1)==0)
#define IS_ALIGNED_4(x) ((x&3)==0)
#define IS_ALIGNED_8(x) ((x&7)==0)
inline_ void _prefetch(void const* ptr) { (void)*(char const volatile *)ptr; }
// Compute implicit coords from an index:
// The idea is to get back 2D coords from a 1D index.
// For example:
//
// 0 1 2 ... nbu-1
// nbu nbu+1 i ...
//
// We have i, we're looking for the equivalent (u=2, v=1) location.
// i = u + v*nbu
// <=> i/nbu = u/nbu + v
// Since 0 <= u < nbu, u/nbu = 0 (integer)
// Hence: v = i/nbu
// Then we simply put it back in the original equation to compute u = i - v*nbu
inline_ void Compute2DCoords(udword& u, udword& v, udword i, udword nbu)
{
v = i / nbu;
u = i - (v * nbu);
}
// In 3D: i = u + v*nbu + w*nbu*nbv
// <=> i/(nbu*nbv) = u/(nbu*nbv) + v/nbv + w
// u/(nbu*nbv) is null since u/nbu was null already.
// v/nbv is null as well for the same reason.
// Hence w = i/(nbu*nbv)
// Then we're left with a 2D problem: i' = i - w*nbu*nbv = u + v*nbu
inline_ void Compute3DCoords(udword& u, udword& v, udword& w, udword i, udword nbu, udword nbu_nbv)
{
w = i / (nbu_nbv);
Compute2DCoords(u, v, i - (w * nbu_nbv), nbu);
}
#endif // __ICEUTILS_H__

696
ode/OPCODE/OPC_AABBCollider.cpp Normal file
View File

@ -0,0 +1,696 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for an AABB collider.
* \file OPC_AABBCollider.cpp
* \author Pierre Terdiman
* \date January, 1st, 2002
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains an AABB-vs-tree collider.
*
* \class AABBCollider
* \author Pierre Terdiman
* \version 1.3
* \date January, 1st, 2002
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Precompiled Header
#include "Stdafx.h"
using namespace Opcode;
#include "OPC_BoxBoxOverlap.h"
#include "OPC_TriBoxOverlap.h"
#define SET_CONTACT(prim_index, flag) \
/* Set contact status */ \
mFlags |= flag; \
mTouchedPrimitives->Add(udword(prim_index));
//! AABB-triangle test
#define AABB_PRIM(prim_index, flag) \
/* Request vertices from the app */ \
VertexPointers VP; mIMesh->GetTriangle(VP, prim_index);\
mLeafVerts[0] = *VP.Vertex[0]; \
mLeafVerts[1] = *VP.Vertex[1]; \
mLeafVerts[2] = *VP.Vertex[2]; \
/* Perform triangle-box overlap test */ \
if(TriBoxOverlap()) \
{ \
SET_CONTACT(prim_index, flag) \
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Constructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
AABBCollider::AABBCollider()
{
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Destructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
AABBCollider::~AABBCollider()
{
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Generic collision query for generic OPCODE models. After the call, access the results:
* - with GetContactStatus()
* - with GetNbTouchedPrimitives()
* - with GetTouchedPrimitives()
*
* \param cache [in/out] a box cache
* \param box [in] collision AABB in world space
* \param model [in] Opcode model to collide with
* \return true if success
* \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool AABBCollider::Collide(AABBCache& cache, const CollisionAABB& box, const Model& model)
{
// Checkings
if(!Setup(&model)) return false;
// Init collision query
if(InitQuery(cache, box)) return true;
if(!model.HasLeafNodes())
{
if(model.IsQuantized())
{
const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree();
// Setup dequantization coeffs
mCenterCoeff = Tree->mCenterCoeff;
mExtentsCoeff = Tree->mExtentsCoeff;
// Perform collision query
if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes());
else _Collide(Tree->GetNodes());
}
else
{
const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree();
// Perform collision query
if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes());
else _Collide(Tree->GetNodes());
}
}
else
{
if(model.IsQuantized())
{
const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree();
// Setup dequantization coeffs
mCenterCoeff = Tree->mCenterCoeff;
mExtentsCoeff = Tree->mExtentsCoeff;
// Perform collision query
if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes());
else _Collide(Tree->GetNodes());
}
else
{
const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree();
// Perform collision query
if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes());
else _Collide(Tree->GetNodes());
}
}
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Initializes a collision query :
* - reset stats & contact status
* - check temporal coherence
*
* \param cache [in/out] a box cache
* \param box [in] AABB in world space
* \return TRUE if we can return immediately
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
BOOL AABBCollider::InitQuery(AABBCache& cache, const CollisionAABB& box)
{
// 1) Call the base method
VolumeCollider::InitQuery();
// 2) Keep track of the query box
mBox = box;
// 3) Setup destination pointer
mTouchedPrimitives = &cache.TouchedPrimitives;
// 4) Special case: 1-triangle meshes [Opcode 1.3]
if(mCurrentModel && mCurrentModel->HasSingleNode())
{
if(!SkipPrimitiveTests())
{
// We simply perform the BV-Prim overlap test each time. We assume single triangle has index 0.
mTouchedPrimitives->Reset();
// Perform overlap test between the unique triangle and the box (and set contact status if needed)
AABB_PRIM(udword(0), OPC_CONTACT)
// Return immediately regardless of status
return TRUE;
}
}
// 5) Check temporal coherence :
if(TemporalCoherenceEnabled())
{
// Here we use temporal coherence
// => check results from previous frame before performing the collision query
if(FirstContactEnabled())
{
// We're only interested in the first contact found => test the unique previously touched face
if(mTouchedPrimitives->GetNbEntries())
{
// Get index of previously touched face = the first entry in the array
udword PreviouslyTouchedFace = mTouchedPrimitives->GetEntry(0);
// Then reset the array:
// - if the overlap test below is successful, the index we'll get added back anyway
// - if it isn't, then the array should be reset anyway for the normal query
mTouchedPrimitives->Reset();
// Perform overlap test between the cached triangle and the box (and set contact status if needed)
AABB_PRIM(PreviouslyTouchedFace, OPC_TEMPORAL_CONTACT)
// Return immediately if possible
if(GetContactStatus()) return TRUE;
}
// else no face has been touched during previous query
// => we'll have to perform a normal query
}
else
{
// We're interested in all contacts =>test the new real box N(ew) against the previous fat box P(revious):
if(IsCacheValid(cache) && mBox.IsInside(cache.FatBox))
{
// - if N is included in P, return previous list
// => we simply leave the list (mTouchedFaces) unchanged
// Set contact status if needed
if(mTouchedPrimitives->GetNbEntries()) mFlags |= OPC_TEMPORAL_CONTACT;
// In any case we don't need to do a query
return TRUE;
}
else
{
// - else do the query using a fat N
// Reset cache since we'll about to perform a real query
mTouchedPrimitives->Reset();
// Make a fat box so that coherence will work for subsequent frames
mBox.mExtents *= cache.FatCoeff;
// Update cache with query data (signature for cached faces)
cache.FatBox = mBox;
}
}
}
else
{
// Here we don't use temporal coherence => do a normal query
mTouchedPrimitives->Reset();
}
// 5) Precompute min & max bounds if needed
mMin = box.mCenter - box.mExtents;
mMax = box.mCenter + box.mExtents;
return FALSE;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Collision query for vanilla AABB trees.
* \param cache [in/out] a box cache
* \param box [in] collision AABB in world space
* \param tree [in] AABB tree
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool AABBCollider::Collide(AABBCache& cache, const CollisionAABB& box, const AABBTree* tree)
{
// This is typically called for a scene tree, full of -AABBs-, not full of triangles.
// So we don't really have "primitives" to deal with. Hence it doesn't work with
// "FirstContact" + "TemporalCoherence".
ASSERT( !(FirstContactEnabled() && TemporalCoherenceEnabled()) );
// Checkings
if(!tree) return false;
// Init collision query
if(InitQuery(cache, box)) return true;
// Perform collision query
_Collide(tree);
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Checks the AABB completely contains the box. In which case we can end the query sooner.
* \param bc [in] box center
* \param be [in] box extents
* \return true if the AABB contains the whole box
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ BOOL AABBCollider::AABBContainsBox(const Point& bc, const Point& be)
{
if(mMin.x > bc.x - be.x) return FALSE;
if(mMin.y > bc.y - be.y) return FALSE;
if(mMin.z > bc.z - be.z) return FALSE;
if(mMax.x < bc.x + be.x) return FALSE;
if(mMax.y < bc.y + be.y) return FALSE;
if(mMax.z < bc.z + be.z) return FALSE;
return TRUE;
}
#define TEST_BOX_IN_AABB(center, extents) \
if(AABBContainsBox(center, extents)) \
{ \
/* Set contact status */ \
mFlags |= OPC_CONTACT; \
_Dump(node); \
return; \
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for normal AABB trees.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void AABBCollider::_Collide(const AABBCollisionNode* node)
{
// Perform AABB-AABB overlap test
if(!AABBAABBOverlap(node->mAABB.mExtents, node->mAABB.mCenter)) return;
TEST_BOX_IN_AABB(node->mAABB.mCenter, node->mAABB.mExtents)
if(node->IsLeaf())
{
AABB_PRIM(node->GetPrimitive(), OPC_CONTACT)
}
else
{
_Collide(node->GetPos());
if(ContactFound()) return;
_Collide(node->GetNeg());
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for normal AABB trees, without primitive tests.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void AABBCollider::_CollideNoPrimitiveTest(const AABBCollisionNode* node)
{
// Perform AABB-AABB overlap test
if(!AABBAABBOverlap(node->mAABB.mExtents, node->mAABB.mCenter)) return;
TEST_BOX_IN_AABB(node->mAABB.mCenter, node->mAABB.mExtents)
if(node->IsLeaf())
{
SET_CONTACT(node->GetPrimitive(), OPC_CONTACT)
}
else
{
_CollideNoPrimitiveTest(node->GetPos());
if(ContactFound()) return;
_CollideNoPrimitiveTest(node->GetNeg());
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for quantized AABB trees.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void AABBCollider::_Collide(const AABBQuantizedNode* node)
{
// Dequantize box
const QuantizedAABB& Box = node->mAABB;
const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
// Perform AABB-AABB overlap test
if(!AABBAABBOverlap(Extents, Center)) return;
TEST_BOX_IN_AABB(Center, Extents)
if(node->IsLeaf())
{
AABB_PRIM(node->GetPrimitive(), OPC_CONTACT)
}
else
{
_Collide(node->GetPos());
if(ContactFound()) return;
_Collide(node->GetNeg());
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for quantized AABB trees, without primitive tests.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void AABBCollider::_CollideNoPrimitiveTest(const AABBQuantizedNode* node)
{
// Dequantize box
const QuantizedAABB& Box = node->mAABB;
const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
// Perform AABB-AABB overlap test
if(!AABBAABBOverlap(Extents, Center)) return;
TEST_BOX_IN_AABB(Center, Extents)
if(node->IsLeaf())
{
SET_CONTACT(node->GetPrimitive(), OPC_CONTACT)
}
else
{
_CollideNoPrimitiveTest(node->GetPos());
if(ContactFound()) return;
_CollideNoPrimitiveTest(node->GetNeg());
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for no-leaf AABB trees.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void AABBCollider::_Collide(const AABBNoLeafNode* node)
{
// Perform AABB-AABB overlap test
if(!AABBAABBOverlap(node->mAABB.mExtents, node->mAABB.mCenter)) return;
TEST_BOX_IN_AABB(node->mAABB.mCenter, node->mAABB.mExtents)
if(node->HasPosLeaf()) { AABB_PRIM(node->GetPosPrimitive(), OPC_CONTACT) }
else _Collide(node->GetPos());
if(ContactFound()) return;
if(node->HasNegLeaf()) { AABB_PRIM(node->GetNegPrimitive(), OPC_CONTACT) }
else _Collide(node->GetNeg());
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for no-leaf AABB trees, without primitive tests.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void AABBCollider::_CollideNoPrimitiveTest(const AABBNoLeafNode* node)
{
// Perform AABB-AABB overlap test
if(!AABBAABBOverlap(node->mAABB.mExtents, node->mAABB.mCenter)) return;
TEST_BOX_IN_AABB(node->mAABB.mCenter, node->mAABB.mExtents)
if(node->HasPosLeaf()) { SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) }
else _CollideNoPrimitiveTest(node->GetPos());
if(ContactFound()) return;
if(node->HasNegLeaf()) { SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) }
else _CollideNoPrimitiveTest(node->GetNeg());
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for quantized no-leaf AABB trees.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void AABBCollider::_Collide(const AABBQuantizedNoLeafNode* node)
{
// Dequantize box
const QuantizedAABB& Box = node->mAABB;
const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
// Perform AABB-AABB overlap test
if(!AABBAABBOverlap(Extents, Center)) return;
TEST_BOX_IN_AABB(Center, Extents)
if(node->HasPosLeaf()) { AABB_PRIM(node->GetPosPrimitive(), OPC_CONTACT) }
else _Collide(node->GetPos());
if(ContactFound()) return;
if(node->HasNegLeaf()) { AABB_PRIM(node->GetNegPrimitive(), OPC_CONTACT) }
else _Collide(node->GetNeg());
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for quantized no-leaf AABB trees, without primitive tests.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void AABBCollider::_CollideNoPrimitiveTest(const AABBQuantizedNoLeafNode* node)
{
// Dequantize box
const QuantizedAABB& Box = node->mAABB;
const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
// Perform AABB-AABB overlap test
if(!AABBAABBOverlap(Extents, Center)) return;
TEST_BOX_IN_AABB(Center, Extents)
if(node->HasPosLeaf()) { SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) }
else _CollideNoPrimitiveTest(node->GetPos());
if(ContactFound()) return;
if(node->HasNegLeaf()) { SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) }
else _CollideNoPrimitiveTest(node->GetNeg());
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for vanilla AABB trees.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void AABBCollider::_Collide(const AABBTreeNode* node)
{
// Perform AABB-AABB overlap test
Point Center, Extents;
node->GetAABB()->GetCenter(Center);
node->GetAABB()->GetExtents(Extents);
if(!AABBAABBOverlap(Center, Extents)) return;
if(node->IsLeaf() || AABBContainsBox(Center, Extents))
{
mFlags |= OPC_CONTACT;
mTouchedPrimitives->Add(node->GetPrimitives(), node->GetNbPrimitives());
}
else
{
_Collide(node->GetPos());
_Collide(node->GetNeg());
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Constructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
HybridAABBCollider::HybridAABBCollider()
{
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Destructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
HybridAABBCollider::~HybridAABBCollider()
{
}
bool HybridAABBCollider::Collide(AABBCache& cache, const CollisionAABB& box, const HybridModel& model)
{
// We don't want primitive tests here!
mFlags |= OPC_NO_PRIMITIVE_TESTS;
// Checkings
if(!Setup(&model)) return false;
// Init collision query
if(InitQuery(cache, box)) return true;
// Special case for 1-leaf trees
if(mCurrentModel && mCurrentModel->HasSingleNode())
{
// Here we're supposed to perform a normal query, except our tree has a single node, i.e. just a few triangles
udword Nb = mIMesh->GetNbTriangles();
// Loop through all triangles
for(udword i=0;i<Nb;i++)
{
AABB_PRIM(i, OPC_CONTACT)
}
return true;
}
// Override destination array since we're only going to get leaf boxes here
mTouchedBoxes.Reset();
mTouchedPrimitives = &mTouchedBoxes;
// Now, do the actual query against leaf boxes
if(!model.HasLeafNodes())
{
if(model.IsQuantized())
{
const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree();
// Setup dequantization coeffs
mCenterCoeff = Tree->mCenterCoeff;
mExtentsCoeff = Tree->mExtentsCoeff;
// Perform collision query - we don't want primitive tests here!
_CollideNoPrimitiveTest(Tree->GetNodes());
}
else
{
const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree();
// Perform collision query - we don't want primitive tests here!
_CollideNoPrimitiveTest(Tree->GetNodes());
}
}
else
{
if(model.IsQuantized())
{
const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree();
// Setup dequantization coeffs
mCenterCoeff = Tree->mCenterCoeff;
mExtentsCoeff = Tree->mExtentsCoeff;
// Perform collision query - we don't want primitive tests here!
_CollideNoPrimitiveTest(Tree->GetNodes());
}
else
{
const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree();
// Perform collision query - we don't want primitive tests here!
_CollideNoPrimitiveTest(Tree->GetNodes());
}
}
// We only have a list of boxes so far
if(GetContactStatus())
{
// Reset contact status, since it currently only reflects collisions with leaf boxes
Collider::InitQuery();
// Change dest container so that we can use built-in overlap tests and get collided primitives
cache.TouchedPrimitives.Reset();
mTouchedPrimitives = &cache.TouchedPrimitives;
// Read touched leaf boxes
udword Nb = mTouchedBoxes.GetNbEntries();
const udword* Touched = mTouchedBoxes.GetEntries();
const LeafTriangles* LT = model.GetLeafTriangles();
const udword* Indices = model.GetIndices();
// Loop through touched leaves
while(Nb--)
{
const LeafTriangles& CurrentLeaf = LT[*Touched++];
// Each leaf box has a set of triangles
udword NbTris = CurrentLeaf.GetNbTriangles();
if(Indices)
{
const udword* T = &Indices[CurrentLeaf.GetTriangleIndex()];
// Loop through triangles and test each of them
while(NbTris--)
{
udword TriangleIndex = *T++;
AABB_PRIM(TriangleIndex, OPC_CONTACT)
}
}
else
{
udword BaseIndex = CurrentLeaf.GetTriangleIndex();
// Loop through triangles and test each of them
while(NbTris--)
{
udword TriangleIndex = BaseIndex++;
AABB_PRIM(TriangleIndex, OPC_CONTACT)
}
}
}
}
return true;
}

97
ode/OPCODE/OPC_AABBCollider.h Normal file
View File

@ -0,0 +1,97 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for an AABB collider.
* \file OPC_AABBCollider.h
* \author Pierre Terdiman
* \date January, 1st, 2002
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __OPC_AABBCOLLIDER_H__
#define __OPC_AABBCOLLIDER_H__
struct OPCODE_API AABBCache : VolumeCache
{
AABBCache() : FatCoeff(1.1f)
{
FatBox.mCenter.Zero();
FatBox.mExtents.Zero();
}
// Cached faces signature
CollisionAABB FatBox; //!< Box used when performing the query resulting in cached faces
// User settings
float FatCoeff; //!< mRadius2 multiplier used to create a fat sphere
};
class OPCODE_API AABBCollider : public VolumeCollider
{
public:
// Constructor / Destructor
AABBCollider();
virtual ~AABBCollider();
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Generic collision query for generic OPCODE models. After the call, access the results:
* - with GetContactStatus()
* - with GetNbTouchedPrimitives()
* - with GetTouchedPrimitives()
*
* \param cache [in/out] a box cache
* \param box [in] collision AABB in world space
* \param model [in] Opcode model to collide with
* \return true if success
* \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool Collide(AABBCache& cache, const CollisionAABB& box, const Model& model);
//
bool Collide(AABBCache& cache, const CollisionAABB& box, const AABBTree* tree);
protected:
CollisionAABB mBox; //!< Query box in (center, extents) form
Point mMin; //!< Query box min point
Point mMax; //!< Query box max point
// Leaf description
Point mLeafVerts[3]; //!< Triangle vertices
// Internal methods
void _Collide(const AABBCollisionNode* node);
void _Collide(const AABBNoLeafNode* node);
void _Collide(const AABBQuantizedNode* node);
void _Collide(const AABBQuantizedNoLeafNode* node);
void _Collide(const AABBTreeNode* node);
void _CollideNoPrimitiveTest(const AABBCollisionNode* node);
void _CollideNoPrimitiveTest(const AABBNoLeafNode* node);
void _CollideNoPrimitiveTest(const AABBQuantizedNode* node);
void _CollideNoPrimitiveTest(const AABBQuantizedNoLeafNode* node);
// Overlap tests
inline_ BOOL AABBContainsBox(const Point& bc, const Point& be);
inline_ BOOL AABBAABBOverlap(const Point& b, const Point& Pb);
inline_ BOOL TriBoxOverlap();
// Init methods
BOOL InitQuery(AABBCache& cache, const CollisionAABB& box);
};
class OPCODE_API HybridAABBCollider : public AABBCollider
{
public:
// Constructor / Destructor
HybridAABBCollider();
virtual ~HybridAABBCollider();
bool Collide(AABBCache& cache, const CollisionAABB& box, const HybridModel& model);
protected:
Container mTouchedBoxes;
};
#endif // __OPC_AABBCOLLIDER_H__

573
ode/OPCODE/OPC_AABBTree.cpp Normal file
View File

@ -0,0 +1,573 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for a versatile AABB tree.
* \file OPC_AABBTree.cpp
* \author Pierre Terdiman
* \date March, 20, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains a generic AABB tree node.
*
* \class AABBTreeNode
* \author Pierre Terdiman
* \version 1.3
* \date March, 20, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains a generic AABB tree.
* This is a vanilla AABB tree, without any particular optimization. It contains anonymous references to
* user-provided primitives, which can theoretically be anything - triangles, boxes, etc. Each primitive
* is surrounded by an AABB, regardless of the primitive's nature. When the primitive is a triangle, the
* resulting tree can be converted into an optimized tree. If the primitive is a box, the resulting tree
* can be used for culling - VFC or occlusion -, assuming you cull on a mesh-by-mesh basis (modern way).
*
* \class AABBTree
* \author Pierre Terdiman
* \version 1.3
* \date March, 20, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Precompiled Header
#include "Stdafx.h"
using namespace Opcode;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Constructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
AABBTreeNode::AABBTreeNode() :
mPos (null),
#ifndef OPC_NO_NEG_VANILLA_TREE
mNeg (null),
#endif
mNbPrimitives (0),
mNodePrimitives (null)
{
#ifdef OPC_USE_TREE_COHERENCE
mBitmask = 0;
#endif
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Destructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
AABBTreeNode::~AABBTreeNode()
{
// Opcode 1.3:
const AABBTreeNode* Pos = GetPos();
const AABBTreeNode* Neg = GetNeg();
#ifndef OPC_NO_NEG_VANILLA_TREE
if(!(mPos&1)) DELETESINGLE(Pos);
if(!(mNeg&1)) DELETESINGLE(Neg);
#else
if(!(mPos&1)) DELETEARRAY(Pos);
#endif
mNodePrimitives = null; // This was just a shortcut to the global list => no release
mNbPrimitives = 0;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Splits the node along a given axis.
* The list of indices is reorganized according to the split values.
* \param axis [in] splitting axis index
* \param builder [in] the tree builder
* \return the number of primitives assigned to the first child
* \warning this method reorganizes the internal list of primitives
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
udword AABBTreeNode::Split(udword axis, AABBTreeBuilder* builder)
{
// Get node split value
float SplitValue = builder->GetSplittingValue(mNodePrimitives, mNbPrimitives, mBV, axis);
udword NbPos = 0;
// Loop through all node-related primitives. Their indices range from mNodePrimitives[0] to mNodePrimitives[mNbPrimitives-1].
// Those indices map the global list in the tree builder.
for(udword i=0;i<mNbPrimitives;i++)
{
// Get index in global list
udword Index = mNodePrimitives[i];
// Test against the splitting value. The primitive value is tested against the enclosing-box center.
// [We only need an approximate partition of the enclosing box here.]
float PrimitiveValue = builder->GetSplittingValue(Index, axis);
// Reorganize the list of indices in this order: positive - negative.
if(PrimitiveValue > SplitValue)
{
// Swap entries
udword Tmp = mNodePrimitives[i];
mNodePrimitives[i] = mNodePrimitives[NbPos];
mNodePrimitives[NbPos] = Tmp;
// Count primitives assigned to positive space
NbPos++;
}
}
return NbPos;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Subdivides the node.
*
* N
* / \
* / \
* N/2 N/2
* / \ / \
* N/4 N/4 N/4 N/4
* (etc)
*
* A well-balanced tree should have a O(log n) depth.
* A degenerate tree would have a O(n) depth.
* Note a perfectly-balanced tree is not well-suited to collision detection anyway.
*
* \param builder [in] the tree builder
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool AABBTreeNode::Subdivide(AABBTreeBuilder* builder)
{
// Checkings
if(!builder) return false;
// Stop subdividing if we reach a leaf node. This is always performed here,
// else we could end in trouble if user overrides this.
if(mNbPrimitives==1) return true;
// Let the user validate the subdivision
if(!builder->ValidateSubdivision(mNodePrimitives, mNbPrimitives, mBV)) return true;
bool ValidSplit = true; // Optimism...
udword NbPos;
if(builder->mSettings.mRules & SPLIT_LARGEST_AXIS)
{
// Find the largest axis to split along
Point Extents; mBV.GetExtents(Extents); // Box extents
udword Axis = Extents.LargestAxis(); // Index of largest axis
// Split along the axis
NbPos = Split(Axis, builder);
// Check split validity
if(!NbPos || NbPos==mNbPrimitives) ValidSplit = false;
}
else if(builder->mSettings.mRules & SPLIT_SPLATTER_POINTS)
{
// Compute the means
Point Means(0.0f, 0.0f, 0.0f);
for(udword i=0;i<mNbPrimitives;i++)
{
udword Index = mNodePrimitives[i];
Means.x+=builder->GetSplittingValue(Index, 0);
Means.y+=builder->GetSplittingValue(Index, 1);
Means.z+=builder->GetSplittingValue(Index, 2);
}
Means/=float(mNbPrimitives);
// Compute variances
Point Vars(0.0f, 0.0f, 0.0f);
for(udword i=0;i<mNbPrimitives;i++)
{
udword Index = mNodePrimitives[i];
float Cx = builder->GetSplittingValue(Index, 0);
float Cy = builder->GetSplittingValue(Index, 1);
float Cz = builder->GetSplittingValue(Index, 2);
Vars.x += (Cx - Means.x)*(Cx - Means.x);
Vars.y += (Cy - Means.y)*(Cy - Means.y);
Vars.z += (Cz - Means.z)*(Cz - Means.z);
}
Vars/=float(mNbPrimitives-1);
// Choose axis with greatest variance
udword Axis = Vars.LargestAxis();
// Split along the axis
NbPos = Split(Axis, builder);
// Check split validity
if(!NbPos || NbPos==mNbPrimitives) ValidSplit = false;
}
else if(builder->mSettings.mRules & SPLIT_BALANCED)
{
// Test 3 axis, take the best
float Results[3];
NbPos = Split(0, builder); Results[0] = float(NbPos)/float(mNbPrimitives);
NbPos = Split(1, builder); Results[1] = float(NbPos)/float(mNbPrimitives);
NbPos = Split(2, builder); Results[2] = float(NbPos)/float(mNbPrimitives);
Results[0]-=0.5f; Results[0]*=Results[0];
Results[1]-=0.5f; Results[1]*=Results[1];
Results[2]-=0.5f; Results[2]*=Results[2];
udword Min=0;
if(Results[1]<Results[Min]) Min = 1;
if(Results[2]<Results[Min]) Min = 2;
// Split along the axis
NbPos = Split(Min, builder);
// Check split validity
if(!NbPos || NbPos==mNbPrimitives) ValidSplit = false;
}
else if(builder->mSettings.mRules & SPLIT_BEST_AXIS)
{
// Test largest, then middle, then smallest axis...
// Sort axis
Point Extents; mBV.GetExtents(Extents); // Box extents
udword SortedAxis[] = { 0, 1, 2 };
float* Keys = (float*)&Extents.x;
for(udword j=0;j<3;j++)
{
for(udword i=0;i<2;i++)
{
if(Keys[SortedAxis[i]]<Keys[SortedAxis[i+1]])
{
udword Tmp = SortedAxis[i];
SortedAxis[i] = SortedAxis[i+1];
SortedAxis[i+1] = Tmp;
}
}
}
// Find the largest axis to split along
udword CurAxis = 0;
ValidSplit = false;
while(!ValidSplit && CurAxis!=3)
{
NbPos = Split(SortedAxis[CurAxis], builder);
// Check the subdivision has been successful
if(!NbPos || NbPos==mNbPrimitives) CurAxis++;
else ValidSplit = true;
}
}
else if(builder->mSettings.mRules & SPLIT_FIFTY)
{
// Don't even bother splitting (mainly a performance test)
NbPos = mNbPrimitives>>1;
}
else return false; // Unknown splitting rules
// Check the subdivision has been successful
if(!ValidSplit)
{
// Here, all boxes lie in the same sub-space. Two strategies:
// - if the tree *must* be complete, make an arbitrary 50-50 split
// - else stop subdividing
// if(builder->mSettings.mRules&SPLIT_COMPLETE)
if(builder->mSettings.mLimit==1)
{
builder->IncreaseNbInvalidSplits();
NbPos = mNbPrimitives>>1;
}
else return true;
}
// Now create children and assign their pointers.
if(builder->mNodeBase)
{
// We use a pre-allocated linear pool for complete trees [Opcode 1.3]
AABBTreeNode* Pool = (AABBTreeNode*)builder->mNodeBase;
udword Count = builder->GetCount() - 1; // Count begins to 1...
// Set last bit to tell it shouldn't be freed ### pretty ugly, find a better way. Maybe one bit in mNbPrimitives
ASSERT(!(udword(&Pool[Count+0])&1));
ASSERT(!(udword(&Pool[Count+1])&1));
mPos = size_t(&Pool[Count+0])|1;
#ifndef OPC_NO_NEG_VANILLA_TREE
mNeg = size_t(&Pool[Count+1])|1;
#endif
}
else
{
// Non-complete trees and/or Opcode 1.2 allocate nodes on-the-fly
#ifndef OPC_NO_NEG_VANILLA_TREE
mPos = (size_t)new AABBTreeNode; CHECKALLOC(mPos);
mNeg = (size_t)new AABBTreeNode; CHECKALLOC(mNeg);
#else
AABBTreeNode* PosNeg = new AABBTreeNode[2];
CHECKALLOC(PosNeg);
mPos = (size_t)PosNeg;
#endif
}
// Update stats
builder->IncreaseCount(2);
// Assign children
AABBTreeNode* Pos = (AABBTreeNode*)GetPos();
AABBTreeNode* Neg = (AABBTreeNode*)GetNeg();
Pos->mNodePrimitives = &mNodePrimitives[0];
Pos->mNbPrimitives = NbPos;
Neg->mNodePrimitives = &mNodePrimitives[NbPos];
Neg->mNbPrimitives = mNbPrimitives - NbPos;
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive hierarchy building in a top-down fashion.
* \param builder [in] the tree builder
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void AABBTreeNode::_BuildHierarchy(AABBTreeBuilder* builder)
{
// 1) Compute the global box for current node. The box is stored in mBV.
builder->ComputeGlobalBox(mNodePrimitives, mNbPrimitives, mBV);
// 2) Subdivide current node
Subdivide(builder);
// 3) Recurse
AABBTreeNode* Pos = (AABBTreeNode*)GetPos();
AABBTreeNode* Neg = (AABBTreeNode*)GetNeg();
if(Pos) Pos->_BuildHierarchy(builder);
if(Neg) Neg->_BuildHierarchy(builder);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Refits the tree (top-down).
* \param builder [in] the tree builder
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void AABBTreeNode::_Refit(AABBTreeBuilder* builder)
{
// 1) Recompute the new global box for current node
builder->ComputeGlobalBox(mNodePrimitives, mNbPrimitives, mBV);
// 2) Recurse
AABBTreeNode* Pos = (AABBTreeNode*)GetPos();
AABBTreeNode* Neg = (AABBTreeNode*)GetNeg();
if(Pos) Pos->_Refit(builder);
if(Neg) Neg->_Refit(builder);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Constructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
AABBTree::AABBTree() : mIndices(null), mTotalNbNodes(0), mPool(null)
{
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Destructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
AABBTree::~AABBTree()
{
Release();
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Releases the tree.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void AABBTree::Release()
{
DELETEARRAY(mPool);
DELETEARRAY(mIndices);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Builds a generic AABB tree from a tree builder.
* \param builder [in] the tree builder
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool AABBTree::Build(AABBTreeBuilder* builder)
{
// Checkings
if(!builder || !builder->mNbPrimitives) return false;
// Release previous tree
Release();
// Init stats
builder->SetCount(1);
builder->SetNbInvalidSplits(0);
// Initialize indices. This list will be modified during build.
mIndices = new udword[builder->mNbPrimitives];
CHECKALLOC(mIndices);
// Identity permutation
for(udword i=0;i<builder->mNbPrimitives;i++) mIndices[i] = i;
// Setup initial node. Here we have a complete permutation of the app's primitives.
mNodePrimitives = mIndices;
mNbPrimitives = builder->mNbPrimitives;
// Use a linear array for complete trees (since we can predict the final number of nodes) [Opcode 1.3]
// if(builder->mRules&SPLIT_COMPLETE)
if(builder->mSettings.mLimit==1)
{
// Allocate a pool of nodes
mPool = new AABBTreeNode[builder->mNbPrimitives*2 - 1];
builder->mNodeBase = mPool; // ### ugly !
}
// Build the hierarchy
_BuildHierarchy(builder);
// Get back total number of nodes
mTotalNbNodes = builder->GetCount();
// For complete trees, check the correct number of nodes has been created [Opcode 1.3]
if(mPool) ASSERT(mTotalNbNodes==builder->mNbPrimitives*2 - 1);
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the depth of the tree.
* A well-balanced tree should have a log(n) depth. A degenerate tree O(n) depth.
* \return depth of the tree
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
udword AABBTree::ComputeDepth() const
{
return Walk(null, null); // Use the walking code without callback
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Walks the tree, calling the user back for each node.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
udword AABBTree::Walk(WalkingCallback callback, void* user_data) const
{
// Call it without callback to compute max depth
udword MaxDepth = 0;
udword CurrentDepth = 0;
struct Local
{
static void _Walk(const AABBTreeNode* current_node, udword& max_depth, udword& current_depth, WalkingCallback callback, void* user_data)
{
// Checkings
if(!current_node) return;
// Entering a new node => increase depth
current_depth++;
// Keep track of max depth
if(current_depth>max_depth) max_depth = current_depth;
// Callback
if(callback && !(callback)(current_node, current_depth, user_data)) return;
// Recurse
if(current_node->GetPos()) { _Walk(current_node->GetPos(), max_depth, current_depth, callback, user_data); current_depth--; }
if(current_node->GetNeg()) { _Walk(current_node->GetNeg(), max_depth, current_depth, callback, user_data); current_depth--; }
}
};
Local::_Walk(this, MaxDepth, CurrentDepth, callback, user_data);
return MaxDepth;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Refits the tree in a top-down way.
* \param builder [in] the tree builder
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool AABBTree::Refit(AABBTreeBuilder* builder)
{
if(!builder) return false;
_Refit(builder);
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Refits the tree in a bottom-up way.
* \param builder [in] the tree builder
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool AABBTree::Refit2(AABBTreeBuilder* builder)
{
// Checkings
if(!builder) return false;
ASSERT(mPool);
// Bottom-up update
Point Min,Max;
Point Min_,Max_;
udword Index = mTotalNbNodes;
while(Index--)
{
AABBTreeNode& Current = mPool[Index];
if(Current.IsLeaf())
{
builder->ComputeGlobalBox(Current.GetPrimitives(), Current.GetNbPrimitives(), *(AABB*)Current.GetAABB());
}
else
{
Current.GetPos()->GetAABB()->GetMin(Min);
Current.GetPos()->GetAABB()->GetMax(Max);
Current.GetNeg()->GetAABB()->GetMin(Min_);
Current.GetNeg()->GetAABB()->GetMax(Max_);
Min.Min(Min_);
Max.Max(Max_);
((AABB*)Current.GetAABB())->SetMinMax(Min, Max);
}
}
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the number of bytes used by the tree.
* \return number of bytes used
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
udword AABBTree::GetUsedBytes() const
{
udword TotalSize = mTotalNbNodes*GetNodeSize();
if(mIndices) TotalSize+=mNbPrimitives*sizeof(udword);
return TotalSize;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Checks the tree is a complete tree or not.
* A complete tree is made of 2*N-1 nodes, where N is the number of primitives in the tree.
* \return true for complete trees
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool AABBTree::IsComplete() const
{
return (GetNbNodes()==GetNbPrimitives()*2-1);
}

137
ode/OPCODE/OPC_AABBTree.h Normal file
View File

@ -0,0 +1,137 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for a versatile AABB tree.
* \file OPC_AABBTree.h
* \author Pierre Terdiman
* \date March, 20, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __OPC_AABBTREE_H__
#define __OPC_AABBTREE_H__
#ifdef OPC_NO_NEG_VANILLA_TREE
//! TO BE DOCUMENTED
#define IMPLEMENT_TREE(base_class, volume) \
public: \
/* Constructor / Destructor */ \
base_class(); \
~base_class(); \
/* Data access */ \
inline_ const volume* Get##volume() const { return &mBV; } \
/* Clear the last bit */ \
inline_ const base_class* GetPos() const { return (const base_class*)(mPos&~1); } \
inline_ const base_class* GetNeg() const { const base_class* P = GetPos(); return P ? P+1 : null;} \
\
/* We don't need to test both nodes since we can't have one without the other */ \
inline_ bool IsLeaf() const { return !GetPos(); } \
\
/* Stats */ \
inline_ udword GetNodeSize() const { return SIZEOFOBJECT; } \
protected: \
/* Tree-independent data */ \
/* Following data always belong to the BV-tree, regardless of what the tree actually contains.*/ \
/* Whatever happens we need the two children and the enclosing volume.*/ \
volume mBV; /* Global bounding-volume enclosing all the node-related primitives */ \
size_t mPos; /* "Positive" & "Negative" children */
#else
//! TO BE DOCUMENTED
#define IMPLEMENT_TREE(base_class, volume) \
public: \
/* Constructor / Destructor */ \
base_class(); \
~base_class(); \
/* Data access */ \
inline_ const volume* Get##volume() const { return &mBV; } \
/* Clear the last bit */ \
inline_ const base_class* GetPos() const { return (const base_class*)(mPos&~1); } \
inline_ const base_class* GetNeg() const { return (const base_class*)(mNeg&~1); } \
\
/* inline_ bool IsLeaf() const { return (!GetPos() && !GetNeg()); } */ \
/* We don't need to test both nodes since we can't have one without the other */ \
inline_ bool IsLeaf() const { return !GetPos(); } \
\
/* Stats */ \
inline_ udword GetNodeSize() const { return SIZEOFOBJECT; } \
protected: \
/* Tree-independent data */ \
/* Following data always belong to the BV-tree, regardless of what the tree actually contains.*/ \
/* Whatever happens we need the two children and the enclosing volume.*/ \
volume mBV; /* Global bounding-volume enclosing all the node-related primitives */ \
size_t mPos; /* "Positive" child */ \
size_t mNeg; /* "Negative" child */
#endif
typedef void (*CullingCallback) (udword nb_primitives, udword* node_primitives, BOOL need_clipping, void* user_data);
class OPCODE_API AABBTreeNode
{
IMPLEMENT_TREE(AABBTreeNode, AABB)
public:
// Data access
inline_ const udword* GetPrimitives() const { return mNodePrimitives; }
inline_ udword GetNbPrimitives() const { return mNbPrimitives; }
protected:
// Tree-dependent data
udword* mNodePrimitives; //!< Node-related primitives (shortcut to a position in mIndices below)
udword mNbPrimitives; //!< Number of primitives for this node
// Internal methods
udword Split(udword axis, AABBTreeBuilder* builder);
bool Subdivide(AABBTreeBuilder* builder);
void _BuildHierarchy(AABBTreeBuilder* builder);
void _Refit(AABBTreeBuilder* builder);
};
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* User-callback, called for each node by the walking code.
* \param current [in] current node
* \param depth [in] current node's depth
* \param user_data [in] user-defined data
* \return true to recurse through children, else false to bypass them
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
typedef bool (*WalkingCallback) (const AABBTreeNode* current, udword depth, void* user_data);
class OPCODE_API AABBTree : public AABBTreeNode
{
public:
// Constructor / Destructor
AABBTree();
~AABBTree();
// Build
bool Build(AABBTreeBuilder* builder);
void Release();
// Data access
inline_ const udword* GetIndices() const { return mIndices; } //!< Catch the indices
inline_ udword GetNbNodes() const { return mTotalNbNodes; } //!< Catch the number of nodes
// Infos
bool IsComplete() const;
// Stats
udword ComputeDepth() const;
udword GetUsedBytes() const;
udword Walk(WalkingCallback callback, void* user_data) const;
bool Refit(AABBTreeBuilder* builder);
bool Refit2(AABBTreeBuilder* builder);
private:
udword* mIndices; //!< Indices in the app list. Indices are reorganized during build (permutation).
AABBTreeNode* mPool; //!< Linear pool of nodes for complete trees. Null otherwise. [Opcode 1.3]
// Stats
udword mTotalNbNodes; //!< Number of nodes in the tree.
};
#endif // __OPC_AABBTREE_H__

138
ode/OPCODE/OPC_BaseModel.cpp Normal file
View File

@ -0,0 +1,138 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains base model interface.
* \file OPC_BaseModel.cpp
* \author Pierre Terdiman
* \date May, 18, 2003
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* The base class for collision models.
*
* \class BaseModel
* \author Pierre Terdiman
* \version 1.3
* \date May, 18, 2003
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Precompiled Header
#include "Stdafx.h"
using namespace Opcode;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Constructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
OPCODECREATE::OPCODECREATE()
{
mIMesh = null;
mSettings.mRules = SPLIT_SPLATTER_POINTS | SPLIT_GEOM_CENTER;
mSettings.mLimit = 1; // Mandatory for complete trees
mNoLeaf = true;
mQuantized = true;
#ifdef __MESHMERIZER_H__
mCollisionHull = false;
#endif // __MESHMERIZER_H__
mKeepOriginal = false;
mCanRemap = false;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Constructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
BaseModel::BaseModel() : mIMesh(null), mModelCode(0), mSource(null), mTree(null)
{
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Destructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
BaseModel::~BaseModel()
{
ReleaseBase();
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Releases everything.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void BaseModel::ReleaseBase()
{
DELETESINGLE(mSource);
DELETESINGLE(mTree);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Creates an optimized tree according to user-settings, and setups mModelCode.
* \param no_leaf [in] true for "no leaf" tree
* \param quantized [in] true for quantized tree
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool BaseModel::CreateTree(bool no_leaf, bool quantized)
{
DELETESINGLE(mTree);
// Setup model code
if(no_leaf) mModelCode |= OPC_NO_LEAF;
else mModelCode &= ~OPC_NO_LEAF;
if(quantized) mModelCode |= OPC_QUANTIZED;
else mModelCode &= ~OPC_QUANTIZED;
// Create the correct class
if(mModelCode & OPC_NO_LEAF)
{
if(mModelCode & OPC_QUANTIZED) mTree = new AABBQuantizedNoLeafTree;
else mTree = new AABBNoLeafTree;
}
else
{
if(mModelCode & OPC_QUANTIZED) mTree = new AABBQuantizedTree;
else mTree = new AABBCollisionTree;
}
CHECKALLOC(mTree);
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Refits the collision model. This can be used to handle dynamic meshes. Usage is:
* 1. modify your mesh vertices (keep the topology constant!)
* 2. refit the tree (call this method)
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool BaseModel::Refit()
{
// Refit the optimized tree
return mTree->Refit(mIMesh);
// Old code kept for reference : refit the source tree then rebuild !
// if(!mSource) return false;
// // Ouch...
// mSource->Refit(&mTB);
// // Ouch...
// return mTree->Build(mSource);
}

175
ode/OPCODE/OPC_BaseModel.h Normal file
View File

@ -0,0 +1,175 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains base model interface.
* \file OPC_BaseModel.h
* \author Pierre Terdiman
* \date May, 18, 2003
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __OPC_BASEMODEL_H__
#define __OPC_BASEMODEL_H__
//! Model creation structure
struct OPCODE_API OPCODECREATE
{
//! Constructor
OPCODECREATE();
MeshInterface* mIMesh; //!< Mesh interface (access to triangles & vertices) (*)
BuildSettings mSettings; //!< Builder's settings
bool mNoLeaf; //!< true => discard leaf nodes (else use a normal tree)
bool mQuantized; //!< true => quantize the tree (else use a normal tree)
#ifdef __MESHMERIZER_H__
bool mCollisionHull; //!< true => use convex hull + GJK
#endif // __MESHMERIZER_H__
bool mKeepOriginal; //!< true => keep a copy of the original tree (debug purpose)
bool mCanRemap; //!< true => allows OPCODE to reorganize client arrays
// (*) This pointer is saved internally and used by OPCODE until collision structures are released,
// so beware of the object's lifetime.
};
enum ModelFlag
{
OPC_QUANTIZED = (1<<0), //!< Compressed/uncompressed tree
OPC_NO_LEAF = (1<<1), //!< Leaf/NoLeaf tree
OPC_SINGLE_NODE = (1<<2) //!< Special case for 1-node models
};
class OPCODE_API BaseModel
{
public:
// Constructor/Destructor
BaseModel();
virtual ~BaseModel();
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Builds a collision model.
* \param create [in] model creation structure
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
virtual bool Build(const OPCODECREATE& create) = 0;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Gets the number of bytes used by the tree.
* \return amount of bytes used
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
virtual udword GetUsedBytes() const = 0;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Refits the collision model. This can be used to handle dynamic meshes. Usage is:
* 1. modify your mesh vertices (keep the topology constant!)
* 2. refit the tree (call this method)
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
virtual bool Refit();
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Gets the source tree.
* \return generic tree
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ const AABBTree* GetSourceTree() const { return mSource; }
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Gets the tree.
* \return the collision tree
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ const AABBOptimizedTree* GetTree() const { return mTree; }
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Gets the tree.
* \return the collision tree
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ AABBOptimizedTree* GetTree() { return mTree; }
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Gets the number of nodes in the tree.
* Should be 2*N-1 for normal trees and N-1 for optimized ones.
* \return number of nodes
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ udword GetNbNodes() const { return mTree->GetNbNodes(); }
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Checks whether the tree has leaf nodes or not.
* \return true if the tree has leaf nodes (normal tree), else false (optimized tree)
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ BOOL HasLeafNodes() const { return !(mModelCode & OPC_NO_LEAF); }
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Checks whether the tree is quantized or not.
* \return true if the tree is quantized
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ BOOL IsQuantized() const { return mModelCode & OPC_QUANTIZED; }
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Checks whether the model has a single node or not. This special case must be handled separately.
* \return true if the model has only 1 node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ BOOL HasSingleNode() const { return mModelCode & OPC_SINGLE_NODE; }
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Gets the model's code.
* \return model's code
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ udword GetModelCode() const { return mModelCode; }
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Gets the mesh interface.
* \return mesh interface
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ const MeshInterface* GetMeshInterface() const { return mIMesh; }
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Sets the mesh interface.
* \param imesh [in] mesh interface
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ void SetMeshInterface(const MeshInterface* imesh) { mIMesh = imesh; }
protected:
const MeshInterface* mIMesh; //!< User-defined mesh interface
udword mModelCode; //!< Model code = combination of ModelFlag(s)
AABBTree* mSource; //!< Original source tree
AABBOptimizedTree* mTree; //!< Optimized tree owned by the model
// Internal methods
void ReleaseBase();
bool CreateTree(bool no_leaf, bool quantized);
};
#endif //__OPC_BASEMODEL_H__

122
ode/OPCODE/OPC_BoxBoxOverlap.h Normal file
View File

@ -0,0 +1,122 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* OBB-OBB overlap test using the separating axis theorem.
* - original code by Gomez / Gamasutra (similar to Gottschalk's one in RAPID)
* - optimized for AABB trees by computing the rotation matrix once (SOLID-fashion)
* - the fabs matrix is precomputed as well and epsilon-tweaked (RAPID-style, we found this almost mandatory)
* - Class III axes can be disabled... (SOLID & Intel fashion)
* - ...or enabled to perform some profiling
* - CPU comparisons used when appropriate
* - lazy evaluation sometimes saves some work in case of early exits (unlike SOLID)
*
* \param ea [in] extents from box A
* \param ca [in] center from box A
* \param eb [in] extents from box B
* \param cb [in] center from box B
* \return true if boxes overlap
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ BOOL AABBTreeCollider::BoxBoxOverlap(const Point& ea, const Point& ca, const Point& eb, const Point& cb)
{
// Stats
mNbBVBVTests++;
float t,t2;
// Class I : A's basis vectors
float Tx = (mR1to0.m[0][0]*cb.x + mR1to0.m[1][0]*cb.y + mR1to0.m[2][0]*cb.z) + mT1to0.x - ca.x;
t = ea.x + eb.x*mAR.m[0][0] + eb.y*mAR.m[1][0] + eb.z*mAR.m[2][0];
if(GREATER(Tx, t)) return FALSE;
float Ty = (mR1to0.m[0][1]*cb.x + mR1to0.m[1][1]*cb.y + mR1to0.m[2][1]*cb.z) + mT1to0.y - ca.y;
t = ea.y + eb.x*mAR.m[0][1] + eb.y*mAR.m[1][1] + eb.z*mAR.m[2][1];
if(GREATER(Ty, t)) return FALSE;
float Tz = (mR1to0.m[0][2]*cb.x + mR1to0.m[1][2]*cb.y + mR1to0.m[2][2]*cb.z) + mT1to0.z - ca.z;
t = ea.z + eb.x*mAR.m[0][2] + eb.y*mAR.m[1][2] + eb.z*mAR.m[2][2];
if(GREATER(Tz, t)) return FALSE;
// Class II : B's basis vectors
t = Tx*mR1to0.m[0][0] + Ty*mR1to0.m[0][1] + Tz*mR1to0.m[0][2]; t2 = ea.x*mAR.m[0][0] + ea.y*mAR.m[0][1] + ea.z*mAR.m[0][2] + eb.x;
if(GREATER(t, t2)) return FALSE;
t = Tx*mR1to0.m[1][0] + Ty*mR1to0.m[1][1] + Tz*mR1to0.m[1][2]; t2 = ea.x*mAR.m[1][0] + ea.y*mAR.m[1][1] + ea.z*mAR.m[1][2] + eb.y;
if(GREATER(t, t2)) return FALSE;
t = Tx*mR1to0.m[2][0] + Ty*mR1to0.m[2][1] + Tz*mR1to0.m[2][2]; t2 = ea.x*mAR.m[2][0] + ea.y*mAR.m[2][1] + ea.z*mAR.m[2][2] + eb.z;
if(GREATER(t, t2)) return FALSE;
// Class III : 9 cross products
// Cool trick: always perform the full test for first level, regardless of settings.
// That way pathological cases (such as the pencils scene) are quickly rejected anyway !
if(mFullBoxBoxTest || mNbBVBVTests==1)
{
t = Tz*mR1to0.m[0][1] - Ty*mR1to0.m[0][2]; t2 = ea.y*mAR.m[0][2] + ea.z*mAR.m[0][1] + eb.y*mAR.m[2][0] + eb.z*mAR.m[1][0]; if(GREATER(t, t2)) return FALSE; // L = A0 x B0
t = Tz*mR1to0.m[1][1] - Ty*mR1to0.m[1][2]; t2 = ea.y*mAR.m[1][2] + ea.z*mAR.m[1][1] + eb.x*mAR.m[2][0] + eb.z*mAR.m[0][0]; if(GREATER(t, t2)) return FALSE; // L = A0 x B1
t = Tz*mR1to0.m[2][1] - Ty*mR1to0.m[2][2]; t2 = ea.y*mAR.m[2][2] + ea.z*mAR.m[2][1] + eb.x*mAR.m[1][0] + eb.y*mAR.m[0][0]; if(GREATER(t, t2)) return FALSE; // L = A0 x B2
t = Tx*mR1to0.m[0][2] - Tz*mR1to0.m[0][0]; t2 = ea.x*mAR.m[0][2] + ea.z*mAR.m[0][0] + eb.y*mAR.m[2][1] + eb.z*mAR.m[1][1]; if(GREATER(t, t2)) return FALSE; // L = A1 x B0
t = Tx*mR1to0.m[1][2] - Tz*mR1to0.m[1][0]; t2 = ea.x*mAR.m[1][2] + ea.z*mAR.m[1][0] + eb.x*mAR.m[2][1] + eb.z*mAR.m[0][1]; if(GREATER(t, t2)) return FALSE; // L = A1 x B1
t = Tx*mR1to0.m[2][2] - Tz*mR1to0.m[2][0]; t2 = ea.x*mAR.m[2][2] + ea.z*mAR.m[2][0] + eb.x*mAR.m[1][1] + eb.y*mAR.m[0][1]; if(GREATER(t, t2)) return FALSE; // L = A1 x B2
t = Ty*mR1to0.m[0][0] - Tx*mR1to0.m[0][1]; t2 = ea.x*mAR.m[0][1] + ea.y*mAR.m[0][0] + eb.y*mAR.m[2][2] + eb.z*mAR.m[1][2]; if(GREATER(t, t2)) return FALSE; // L = A2 x B0
t = Ty*mR1to0.m[1][0] - Tx*mR1to0.m[1][1]; t2 = ea.x*mAR.m[1][1] + ea.y*mAR.m[1][0] + eb.x*mAR.m[2][2] + eb.z*mAR.m[0][2]; if(GREATER(t, t2)) return FALSE; // L = A2 x B1
t = Ty*mR1to0.m[2][0] - Tx*mR1to0.m[2][1]; t2 = ea.x*mAR.m[2][1] + ea.y*mAR.m[2][0] + eb.x*mAR.m[1][2] + eb.y*mAR.m[0][2]; if(GREATER(t, t2)) return FALSE; // L = A2 x B2
}
return TRUE;
}
//! A dedicated version when one box is constant
inline_ BOOL OBBCollider::BoxBoxOverlap(const Point& extents, const Point& center)
{
// Stats
mNbVolumeBVTests++;
float t,t2;
// Class I : A's basis vectors
float Tx = mTBoxToModel.x - center.x; t = extents.x + mBBx1; if(GREATER(Tx, t)) return FALSE;
float Ty = mTBoxToModel.y - center.y; t = extents.y + mBBy1; if(GREATER(Ty, t)) return FALSE;
float Tz = mTBoxToModel.z - center.z; t = extents.z + mBBz1; if(GREATER(Tz, t)) return FALSE;
// Class II : B's basis vectors
t = Tx*mRBoxToModel.m[0][0] + Ty*mRBoxToModel.m[0][1] + Tz*mRBoxToModel.m[0][2];
t2 = extents.x*mAR.m[0][0] + extents.y*mAR.m[0][1] + extents.z*mAR.m[0][2] + mBoxExtents.x;
if(GREATER(t, t2)) return FALSE;
t = Tx*mRBoxToModel.m[1][0] + Ty*mRBoxToModel.m[1][1] + Tz*mRBoxToModel.m[1][2];
t2 = extents.x*mAR.m[1][0] + extents.y*mAR.m[1][1] + extents.z*mAR.m[1][2] + mBoxExtents.y;
if(GREATER(t, t2)) return FALSE;
t = Tx*mRBoxToModel.m[2][0] + Ty*mRBoxToModel.m[2][1] + Tz*mRBoxToModel.m[2][2];
t2 = extents.x*mAR.m[2][0] + extents.y*mAR.m[2][1] + extents.z*mAR.m[2][2] + mBoxExtents.z;
if(GREATER(t, t2)) return FALSE;
// Class III : 9 cross products
// Cool trick: always perform the full test for first level, regardless of settings.
// That way pathological cases (such as the pencils scene) are quickly rejected anyway !
if(mFullBoxBoxTest || mNbVolumeBVTests==1)
{
t = Tz*mRBoxToModel.m[0][1] - Ty*mRBoxToModel.m[0][2]; t2 = extents.y*mAR.m[0][2] + extents.z*mAR.m[0][1] + mBB_1; if(GREATER(t, t2)) return FALSE; // L = A0 x B0
t = Tz*mRBoxToModel.m[1][1] - Ty*mRBoxToModel.m[1][2]; t2 = extents.y*mAR.m[1][2] + extents.z*mAR.m[1][1] + mBB_2; if(GREATER(t, t2)) return FALSE; // L = A0 x B1
t = Tz*mRBoxToModel.m[2][1] - Ty*mRBoxToModel.m[2][2]; t2 = extents.y*mAR.m[2][2] + extents.z*mAR.m[2][1] + mBB_3; if(GREATER(t, t2)) return FALSE; // L = A0 x B2
t = Tx*mRBoxToModel.m[0][2] - Tz*mRBoxToModel.m[0][0]; t2 = extents.x*mAR.m[0][2] + extents.z*mAR.m[0][0] + mBB_4; if(GREATER(t, t2)) return FALSE; // L = A1 x B0
t = Tx*mRBoxToModel.m[1][2] - Tz*mRBoxToModel.m[1][0]; t2 = extents.x*mAR.m[1][2] + extents.z*mAR.m[1][0] + mBB_5; if(GREATER(t, t2)) return FALSE; // L = A1 x B1
t = Tx*mRBoxToModel.m[2][2] - Tz*mRBoxToModel.m[2][0]; t2 = extents.x*mAR.m[2][2] + extents.z*mAR.m[2][0] + mBB_6; if(GREATER(t, t2)) return FALSE; // L = A1 x B2
t = Ty*mRBoxToModel.m[0][0] - Tx*mRBoxToModel.m[0][1]; t2 = extents.x*mAR.m[0][1] + extents.y*mAR.m[0][0] + mBB_7; if(GREATER(t, t2)) return FALSE; // L = A2 x B0
t = Ty*mRBoxToModel.m[1][0] - Tx*mRBoxToModel.m[1][1]; t2 = extents.x*mAR.m[1][1] + extents.y*mAR.m[1][0] + mBB_8; if(GREATER(t, t2)) return FALSE; // L = A2 x B1
t = Ty*mRBoxToModel.m[2][0] - Tx*mRBoxToModel.m[2][1]; t2 = extents.x*mAR.m[2][1] + extents.y*mAR.m[2][0] + mBB_9; if(GREATER(t, t2)) return FALSE; // L = A2 x B2
}
return TRUE;
}
//! A special version for 2 axis-aligned boxes
inline_ BOOL AABBCollider::AABBAABBOverlap(const Point& extents, const Point& center)
{
// Stats
mNbVolumeBVTests++;
float tx = mBox.mCenter.x - center.x; float ex = extents.x + mBox.mExtents.x; if(GREATER(tx, ex)) return FALSE;
float ty = mBox.mCenter.y - center.y; float ey = extents.y + mBox.mExtents.y; if(GREATER(ty, ey)) return FALSE;
float tz = mBox.mCenter.z - center.z; float ez = extents.z + mBox.mExtents.z; if(GREATER(tz, ez)) return FALSE;
return TRUE;
}

367
ode/OPCODE/OPC_BoxPruning.cpp Normal file
View File

@ -0,0 +1,367 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for box pruning.
* \file IceBoxPruning.cpp
* \author Pierre Terdiman
* \date January, 29, 2000
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
You could use a complex sweep-and-prune as implemented in I-Collide.
You could use a complex hashing scheme as implemented in V-Clip or recently in ODE it seems.
You could use a "Recursive Dimensional Clustering" algorithm as implemented in GPG2.
Or you could use this.
Faster ? I don't know. Probably not. It would be a shame. But who knows ?
Easier ? Definitely. Enjoy the sheer simplicity.
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Precompiled Header
#include "Stdafx.h"
using namespace Opcode;
inline_ void FindRunningIndex(udword& index, float* array, udword* sorted, int last, float max)
{
int First=index;
while(First<=last)
{
index = (First+last)>>1;
if(max>array[sorted[index]]) First = index+1;
else last = index-1;
}
}
// ### could be log(n) !
// and maybe use cmp integers
// InsertionSort has better coherence, RadixSort is better for one-shot queries.
#define PRUNING_SORTER RadixSort
//#define PRUNING_SORTER InsertionSort
// Static for coherence
static PRUNING_SORTER* gCompletePruningSorter = null;
static PRUNING_SORTER* gBipartitePruningSorter0 = null;
static PRUNING_SORTER* gBipartitePruningSorter1 = null;
inline_ PRUNING_SORTER* GetCompletePruningSorter()
{
if(!gCompletePruningSorter) gCompletePruningSorter = new PRUNING_SORTER;
return gCompletePruningSorter;
}
inline_ PRUNING_SORTER* GetBipartitePruningSorter0()
{
if(!gBipartitePruningSorter0) gBipartitePruningSorter0 = new PRUNING_SORTER;
return gBipartitePruningSorter0;
}
inline_ PRUNING_SORTER* GetBipartitePruningSorter1()
{
if(!gBipartitePruningSorter1) gBipartitePruningSorter1 = new PRUNING_SORTER;
return gBipartitePruningSorter1;
}
void ReleasePruningSorters()
{
DELETESINGLE(gBipartitePruningSorter1);
DELETESINGLE(gBipartitePruningSorter0);
DELETESINGLE(gCompletePruningSorter);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Bipartite box pruning. Returns a list of overlapping pairs of boxes, each box of the pair belongs to a different set.
* \param nb0 [in] number of boxes in the first set
* \param array0 [in] array of boxes for the first set
* \param nb1 [in] number of boxes in the second set
* \param array1 [in] array of boxes for the second set
* \param pairs [out] array of overlapping pairs
* \param axes [in] projection order (0,2,1 is often best)
* \return true if success.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool Opcode::BipartiteBoxPruning(udword nb0, const AABB** array0, udword nb1, const AABB** array1, Pairs& pairs, const Axes& axes)
{
// Checkings
if(!nb0 || !array0 || !nb1 || !array1) return false;
// Catch axes
udword Axis0 = axes.mAxis0;
udword Axis1 = axes.mAxis1;
udword Axis2 = axes.mAxis2;
// Allocate some temporary data
float* MinPosList0 = new float[nb0];
float* MinPosList1 = new float[nb1];
// 1) Build main lists using the primary axis
for(udword i=0;i<nb0;i++) MinPosList0[i] = array0[i]->GetMin(Axis0);
for(udword i=0;i<nb1;i++) MinPosList1[i] = array1[i]->GetMin(Axis0);
// 2) Sort the lists
PRUNING_SORTER* RS0 = GetBipartitePruningSorter0();
PRUNING_SORTER* RS1 = GetBipartitePruningSorter1();
const udword* Sorted0 = RS0->Sort(MinPosList0, nb0).GetRanks();
const udword* Sorted1 = RS1->Sort(MinPosList1, nb1).GetRanks();
// 3) Prune the lists
udword Index0, Index1;
const udword* const LastSorted0 = &Sorted0[nb0];
const udword* const LastSorted1 = &Sorted1[nb1];
const udword* RunningAddress0 = Sorted0;
const udword* RunningAddress1 = Sorted1;
while(RunningAddress1<LastSorted1 && Sorted0<LastSorted0)
{
Index0 = *Sorted0++;
while(RunningAddress1<LastSorted1 && MinPosList1[*RunningAddress1]<MinPosList0[Index0]) RunningAddress1++;
const udword* RunningAddress2_1 = RunningAddress1;
while(RunningAddress2_1<LastSorted1 && MinPosList1[Index1 = *RunningAddress2_1++]<=array0[Index0]->GetMax(Axis0))
{
if(array0[Index0]->Intersect(*array1[Index1], Axis1))
{
if(array0[Index0]->Intersect(*array1[Index1], Axis2))
{
pairs.AddPair(Index0, Index1);
}
}
}
}
////
while(RunningAddress0<LastSorted0 && Sorted1<LastSorted1)
{
Index0 = *Sorted1++;
while(RunningAddress0<LastSorted0 && MinPosList0[*RunningAddress0]<=MinPosList1[Index0]) RunningAddress0++;
const udword* RunningAddress2_0 = RunningAddress0;
while(RunningAddress2_0<LastSorted0 && MinPosList0[Index1 = *RunningAddress2_0++]<=array1[Index0]->GetMax(Axis0))
{
if(array0[Index1]->Intersect(*array1[Index0], Axis1))
{
if(array0[Index1]->Intersect(*array1[Index0], Axis2))
{
pairs.AddPair(Index1, Index0);
}
}
}
}
DELETEARRAY(MinPosList1);
DELETEARRAY(MinPosList0);
return true;
}
#define ORIGINAL_VERSION
//#define JOAKIM
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Complete box pruning. Returns a list of overlapping pairs of boxes, each box of the pair belongs to the same set.
* \param nb [in] number of boxes
* \param array [in] array of boxes
* \param pairs [out] array of overlapping pairs
* \param axes [in] projection order (0,2,1 is often best)
* \return true if success.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool Opcode::CompleteBoxPruning(udword nb, const AABB** array, Pairs& pairs, const Axes& axes)
{
// Checkings
if(!nb || !array) return false;
// Catch axes
udword Axis0 = axes.mAxis0;
udword Axis1 = axes.mAxis1;
udword Axis2 = axes.mAxis2;
#ifdef ORIGINAL_VERSION
// Allocate some temporary data
// float* PosList = new float[nb];
float* PosList = new float[nb+1];
// 1) Build main list using the primary axis
for(udword i=0;i<nb;i++) PosList[i] = array[i]->GetMin(Axis0);
PosList[nb++] = MAX_FLOAT;
// 2) Sort the list
PRUNING_SORTER* RS = GetCompletePruningSorter();
const udword* Sorted = RS->Sort(PosList, nb).GetRanks();
// 3) Prune the list
const udword* const LastSorted = &Sorted[nb];
const udword* RunningAddress = Sorted;
udword Index0, Index1;
while(RunningAddress<LastSorted && Sorted<LastSorted)
{
Index0 = *Sorted++;
// while(RunningAddress<LastSorted && PosList[*RunningAddress++]<PosList[Index0]);
while(PosList[*RunningAddress++]<PosList[Index0]);
if(RunningAddress<LastSorted)
{
const udword* RunningAddress2 = RunningAddress;
// while(RunningAddress2<LastSorted && PosList[Index1 = *RunningAddress2++]<=array[Index0]->GetMax(Axis0))
while(PosList[Index1 = *RunningAddress2++]<=array[Index0]->GetMax(Axis0))
{
// if(Index0!=Index1)
// {
if(array[Index0]->Intersect(*array[Index1], Axis1))
{
if(array[Index0]->Intersect(*array[Index1], Axis2))
{
pairs.AddPair(Index0, Index1);
}
}
// }
}
}
}
DELETEARRAY(PosList);
#endif
#ifdef JOAKIM
// Allocate some temporary data
// float* PosList = new float[nb];
float* MinList = new float[nb+1];
// 1) Build main list using the primary axis
for(udword i=0;i<nb;i++) MinList[i] = array[i]->GetMin(Axis0);
MinList[nb] = MAX_FLOAT;
// 2) Sort the list
PRUNING_SORTER* RS = GetCompletePruningSorter();
udword* Sorted = RS->Sort(MinList, nb+1).GetRanks();
// 3) Prune the list
// const udword* const LastSorted = &Sorted[nb];
// const udword* const LastSorted = &Sorted[nb-1];
const udword* RunningAddress = Sorted;
udword Index0, Index1;
// while(RunningAddress<LastSorted && Sorted<LastSorted)
// while(RunningAddress<LastSorted)
while(RunningAddress<&Sorted[nb])
// while(Sorted<LastSorted)
{
// Index0 = *Sorted++;
Index0 = *RunningAddress++;
// while(RunningAddress<LastSorted && PosList[*RunningAddress++]<PosList[Index0]);
// while(PosList[*RunningAddress++]<PosList[Index0]);
//RunningAddress = Sorted;
// if(RunningAddress<LastSorted)
{
const udword* RunningAddress2 = RunningAddress;
// while(RunningAddress2<LastSorted && PosList[Index1 = *RunningAddress2++]<=array[Index0]->GetMax(Axis0))
// float CurrentMin = array[Index0]->GetMin(Axis0);
float CurrentMax = array[Index0]->GetMax(Axis0);
while(MinList[Index1 = *RunningAddress2] <= CurrentMax)
// while(PosList[Index1 = *RunningAddress] <= CurrentMax)
{
// if(Index0!=Index1)
// {
if(array[Index0]->Intersect(*array[Index1], Axis1))
{
if(array[Index0]->Intersect(*array[Index1], Axis2))
{
pairs.AddPair(Index0, Index1);
}
}
// }
RunningAddress2++;
// RunningAddress++;
}
}
}
DELETEARRAY(MinList);
#endif
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Brute-force versions are kept:
// - to check the optimized versions return the correct list of intersections
// - to check the speed of the optimized code against the brute-force one
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Brute-force bipartite box pruning. Returns a list of overlapping pairs of boxes, each box of the pair belongs to a different set.
* \param nb0 [in] number of boxes in the first set
* \param array0 [in] array of boxes for the first set
* \param nb1 [in] number of boxes in the second set
* \param array1 [in] array of boxes for the second set
* \param pairs [out] array of overlapping pairs
* \return true if success.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool Opcode::BruteForceBipartiteBoxTest(udword nb0, const AABB** array0, udword nb1, const AABB** array1, Pairs& pairs)
{
// Checkings
if(!nb0 || !array0 || !nb1 || !array1) return false;
// Brute-force nb0*nb1 overlap tests
for(udword i=0;i<nb0;i++)
{
for(udword j=0;j<nb1;j++)
{
if(array0[i]->Intersect(*array1[j])) pairs.AddPair(i, j);
}
}
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Complete box pruning. Returns a list of overlapping pairs of boxes, each box of the pair belongs to the same set.
* \param nb [in] number of boxes
* \param array [in] array of boxes
* \param pairs [out] array of overlapping pairs
* \return true if success.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool Opcode::BruteForceCompleteBoxTest(udword nb, const AABB** array, Pairs& pairs)
{
// Checkings
if(!nb || !array) return false;
// Brute-force n(n-1)/2 overlap tests
for(udword i=0;i<nb;i++)
{
for(udword j=i+1;j<nb;j++)
{
if(array[i]->Intersect(*array[j])) pairs.AddPair(i, j);
}
}
return true;
}

31
ode/OPCODE/OPC_BoxPruning.h Normal file
View File

@ -0,0 +1,31 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for box pruning.
* \file IceBoxPruning.h
* \author Pierre Terdiman
* \date January, 29, 2000
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __OPC_BOXPRUNING_H__
#define __OPC_BOXPRUNING_H__
// Optimized versions
FUNCTION OPCODE_API bool CompleteBoxPruning(udword nb, const AABB** array, Pairs& pairs, const Axes& axes);
FUNCTION OPCODE_API bool BipartiteBoxPruning(udword nb0, const AABB** array0, udword nb1, const AABB** array1, Pairs& pairs, const Axes& axes);
// Brute-force versions
FUNCTION OPCODE_API bool BruteForceCompleteBoxTest(udword nb, const AABB** array, Pairs& pairs);
FUNCTION OPCODE_API bool BruteForceBipartiteBoxTest(udword nb0, const AABB** array0, udword nb1, const AABB** array1, Pairs& pairs);
#endif //__OPC_BOXPRUNING_H__

54
ode/OPCODE/OPC_Collider.cpp Normal file
View File

@ -0,0 +1,54 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains base collider class.
* \file OPC_Collider.cpp
* \author Pierre Terdiman
* \date June, 2, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains the abstract class for colliders.
*
* \class Collider
* \author Pierre Terdiman
* \version 1.3
* \date June, 2, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Precompiled Header
#include "Stdafx.h"
using namespace Opcode;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Constructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Collider::Collider() :
mFlags (0),
mCurrentModel (null),
mIMesh (null)
{
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Destructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Collider::~Collider()
{
}

176
ode/OPCODE/OPC_Collider.h Normal file
View File

@ -0,0 +1,176 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains base collider class.
* \file OPC_Collider.h
* \author Pierre Terdiman
* \date June, 2, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __OPC_COLLIDER_H__
#define __OPC_COLLIDER_H__
enum CollisionFlag
{
OPC_FIRST_CONTACT = (1<<0), //!< Report all contacts (false) or only first one (true)
OPC_TEMPORAL_COHERENCE = (1<<1), //!< Use temporal coherence or not
OPC_CONTACT = (1<<2), //!< Final contact status after a collision query
OPC_TEMPORAL_HIT = (1<<3), //!< There has been an early exit due to temporal coherence
OPC_NO_PRIMITIVE_TESTS = (1<<4), //!< Keep or discard primitive-bv tests in leaf nodes (volume-mesh queries)
OPC_CONTACT_FOUND = OPC_FIRST_CONTACT | OPC_CONTACT,
OPC_TEMPORAL_CONTACT = OPC_TEMPORAL_HIT | OPC_CONTACT,
OPC_FORCE_DWORD = 0x7fffffff
};
class OPCODE_API Collider
{
public:
// Constructor / Destructor
Collider();
virtual ~Collider();
// Collision report
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Gets the last collision status after a collision query.
* \return true if a collision occured
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ BOOL GetContactStatus() const { return mFlags & OPC_CONTACT; }
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Gets the "first contact" mode.
* \return true if "first contact" mode is on
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ BOOL FirstContactEnabled() const { return mFlags & OPC_FIRST_CONTACT; }
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Gets the temporal coherence mode.
* \return true if temporal coherence is on
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ BOOL TemporalCoherenceEnabled() const { return mFlags & OPC_TEMPORAL_COHERENCE; }
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Checks a first contact has already been found.
* \return true if a first contact has been found and we can stop a query
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ BOOL ContactFound() const { return (mFlags&OPC_CONTACT_FOUND)==OPC_CONTACT_FOUND; }
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Checks there's been an early exit due to temporal coherence;
* \return true if a temporal hit has occured
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ BOOL TemporalHit() const { return mFlags & OPC_TEMPORAL_HIT; }
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Checks primitive tests are enabled;
* \return true if primitive tests must be skipped
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ BOOL SkipPrimitiveTests() const { return mFlags & OPC_NO_PRIMITIVE_TESTS; }
// Settings
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Reports all contacts (false) or first contact only (true)
* \param flag [in] true for first contact, false for all contacts
* \see SetTemporalCoherence(bool flag)
* \see ValidateSettings()
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ void SetFirstContact(bool flag)
{
if(flag) mFlags |= OPC_FIRST_CONTACT;
else mFlags &= ~OPC_FIRST_CONTACT;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Enable/disable temporal coherence.
* \param flag [in] true to enable temporal coherence, false to discard it
* \see SetFirstContact(bool flag)
* \see ValidateSettings()
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ void SetTemporalCoherence(bool flag)
{
if(flag) mFlags |= OPC_TEMPORAL_COHERENCE;
else mFlags &= ~OPC_TEMPORAL_COHERENCE;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Enable/disable primitive tests.
* \param flag [in] true to enable primitive tests, false to discard them
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ void SetPrimitiveTests(bool flag)
{
if(!flag) mFlags |= OPC_NO_PRIMITIVE_TESTS;
else mFlags &= ~OPC_NO_PRIMITIVE_TESTS;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Validates current settings. You should call this method after all the settings / callbacks have been defined for a collider.
* \return null if everything is ok, else a string describing the problem
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
virtual const char* ValidateSettings() = 0;
protected:
udword mFlags; //!< Bit flags
const BaseModel* mCurrentModel; //!< Current model for collision query (owner of touched faces)
// User mesh interface
const MeshInterface* mIMesh; //!< User-defined mesh interface
// Internal methods
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Setups current collision model
* \param model [in] current collision model
* \return TRUE if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ BOOL Setup(const BaseModel* model)
{
// Keep track of current model
mCurrentModel = model;
if(!mCurrentModel) return FALSE;
mIMesh = model->GetMeshInterface();
return mIMesh!=null;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Initializes a query
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
virtual inline_ void InitQuery() { mFlags &= ~OPC_TEMPORAL_CONTACT; }
};
#endif // __OPC_COLLIDER_H__

48
ode/OPCODE/OPC_Common.cpp Normal file
View File

@ -0,0 +1,48 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains common classes & defs used in OPCODE.
* \file OPC_Common.cpp
* \author Pierre Terdiman
* \date March, 20, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* An AABB dedicated to collision detection.
* We don't use the generic AABB class included in ICE, since it can be a Min/Max or a Center/Extents one (depends
* on compilation flags). Since the Center/Extents model is more efficient in collision detection, it was worth
* using an extra special class.
*
* \class CollisionAABB
* \author Pierre Terdiman
* \version 1.3
* \date March, 20, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* A quantized AABB.
* Center/Extent model, using 16-bits integers.
*
* \class QuantizedAABB
* \author Pierre Terdiman
* \version 1.3
* \date March, 20, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Precompiled Header
#include "Stdafx.h"
using namespace Opcode;

101
ode/OPCODE/OPC_Common.h Normal file
View File

@ -0,0 +1,101 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains common classes & defs used in OPCODE.
* \file OPC_Common.h
* \author Pierre Terdiman
* \date March, 20, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __OPC_COMMON_H__
#define __OPC_COMMON_H__
// [GOTTFRIED]: Just a small change for readability.
#ifdef OPC_CPU_COMPARE
#define GREATER(x, y) AIR(x) > IR(y)
#else
#define GREATER(x, y) fabsf(x) > (y)
#endif
class OPCODE_API CollisionAABB
{
public:
//! Constructor
inline_ CollisionAABB() {}
//! Constructor
inline_ CollisionAABB(const AABB& b) { b.GetCenter(mCenter); b.GetExtents(mExtents); }
//! Destructor
inline_ ~CollisionAABB() {}
//! Get min point of the box
inline_ void GetMin(Point& min) const { min = mCenter - mExtents; }
//! Get max point of the box
inline_ void GetMax(Point& max) const { max = mCenter + mExtents; }
//! Get component of the box's min point along a given axis
inline_ float GetMin(udword axis) const { return mCenter[axis] - mExtents[axis]; }
//! Get component of the box's max point along a given axis
inline_ float GetMax(udword axis) const { return mCenter[axis] + mExtents[axis]; }
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Setups an AABB from min & max vectors.
* \param min [in] the min point
* \param max [in] the max point
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ void SetMinMax(const Point& min, const Point& max) { mCenter = (max + min)*0.5f; mExtents = (max - min)*0.5f; }
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Checks a box is inside another box.
* \param box [in] the other box
* \return true if current box is inside input box
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ BOOL IsInside(const CollisionAABB& box) const
{
if(box.GetMin(0)>GetMin(0)) return FALSE;
if(box.GetMin(1)>GetMin(1)) return FALSE;
if(box.GetMin(2)>GetMin(2)) return FALSE;
if(box.GetMax(0)<GetMax(0)) return FALSE;
if(box.GetMax(1)<GetMax(1)) return FALSE;
if(box.GetMax(2)<GetMax(2)) return FALSE;
return TRUE;
}
Point mCenter; //!< Box center
Point mExtents; //!< Box extents
};
class OPCODE_API QuantizedAABB
{
public:
//! Constructor
inline_ QuantizedAABB() {}
//! Destructor
inline_ ~QuantizedAABB() {}
sword mCenter[3]; //!< Quantized center
uword mExtents[3]; //!< Quantized extents
};
//! Quickly rotates & translates a vector
inline_ void TransformPoint(Point& dest, const Point& source, const Matrix3x3& rot, const Point& trans)
{
dest.x = trans.x + source.x * rot.m[0][0] + source.y * rot.m[1][0] + source.z * rot.m[2][0];
dest.y = trans.y + source.x * rot.m[0][1] + source.y * rot.m[1][1] + source.z * rot.m[2][1];
dest.z = trans.z + source.x * rot.m[0][2] + source.y * rot.m[1][2] + source.z * rot.m[2][2];
}
#endif //__OPC_COMMON_H__

466
ode/OPCODE/OPC_HybridModel.cpp Normal file
View File

@ -0,0 +1,466 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for hybrid models.
* \file OPC_HybridModel.cpp
* \author Pierre Terdiman
* \date May, 18, 2003
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* An hybrid collision model.
*
* The problem :
*
* Opcode really shines for mesh-mesh collision, especially when meshes are deeply overlapping
* (it typically outperforms RAPID in those cases).
*
* Unfortunately this is not the typical scenario in games.
*
* For close-proximity cases, especially for volume-mesh queries, it's relatively easy to run faster
* than Opcode, that suffers from a relatively high setup time.
*
* In particular, Opcode's "vanilla" trees in those cases -can- run faster. They can also use -less-
* memory than the optimized ones, when you let the system stop at ~10 triangles / leaf for example
* (i.e. when you don't use "complete" trees). However, those trees tend to fragment memory quite a
* lot, increasing cache misses : since they're not "complete", we can't predict the final number of
* nodes and we have to allocate nodes on-the-fly. For the same reasons we can't use Opcode's "optimized"
* trees here, since they rely on a known layout to perform the "optimization".
*
* Hybrid trees :
*
* Hybrid trees try to combine best of both worlds :
*
* - they use a maximum limit of 16 triangles/leaf. "16" is used so that we'll be able to save the
* number of triangles using 4 bits only.
*
* - they're still "complete" trees thanks to a two-passes building phase. First we create a "vanilla"
* AABB-tree with Opcode, limited to 16 triangles/leaf. Then we create a *second* vanilla tree, this
* time using the leaves of the first one. The trick is : this second tree is now "complete"... so we
* can further transform it into an Opcode's optimized tree.
*
* - then we run the collision queries on that standard Opcode tree. The only difference is that leaf
* nodes contain indices to leaf nodes of another tree. Also, we have to skip all primitive tests in
* Opcode optimized trees, since our leaves don't contain triangles anymore.
*
* - finally, for each collided leaf, we simply loop through 16 triangles max, and collide them with
* the bounding volume used in the query (we only support volume-vs-mesh queries here, not mesh-vs-mesh)
*
* All of that is wrapped in this "hybrid model" that contains the minimal data required for this to work.
* It's a mix between old "vanilla" trees, and old "optimized" trees.
*
* Extra advantages:
*
* - If we use them for dynamic models, we're left with a very small number of leaf nodes to refit. It
* might be a bit faster since we have less nodes to write back.
*
* - In rigid body simulation, using temporal coherence and sleeping objects greatly reduce the actual
* influence of one tree over another (i.e. the speed difference is often invisible). So memory is really
* the key element to consider, and in this regard hybrid trees are just better.
*
* Information to take home:
* - they use less ram
* - they're not slower (they're faster or slower depending on cases, overall there's no significant
* difference *as long as objects don't interpenetrate too much* - in which case Opcode's optimized trees
* are still notably faster)
*
* \class HybridModel
* \author Pierre Terdiman
* \version 1.3
* \date May, 18, 2003
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Precompiled Header
#include "Stdafx.h"
using namespace Opcode;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Constructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
HybridModel::HybridModel() :
mNbLeaves (0),
mNbPrimitives (0),
mTriangles (null),
mIndices (null)
{
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Destructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
HybridModel::~HybridModel()
{
Release();
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Releases everything.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void HybridModel::Release()
{
ReleaseBase();
DELETEARRAY(mIndices);
DELETEARRAY(mTriangles);
mNbLeaves = 0;
mNbPrimitives = 0;
}
struct Internal
{
Internal()
{
mNbLeaves = 0;
mLeaves = null;
mTriangles = null;
mBase = null;
}
~Internal()
{
DELETEARRAY(mLeaves);
}
udword mNbLeaves;
AABB* mLeaves;
LeafTriangles* mTriangles;
const udword* mBase;
};
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Builds a collision model.
* \param create [in] model creation structure
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool HybridModel::Build(const OPCODECREATE& create)
{
// 1) Checkings
if(!create.mIMesh || !create.mIMesh->IsValid()) return false;
// Look for degenerate faces.
udword NbDegenerate = create.mIMesh->CheckTopology();
if(NbDegenerate) Log("OPCODE WARNING: found %d degenerate faces in model! Collision might report wrong results!\n", NbDegenerate);
// We continue nonetheless....
Release(); // Make sure previous tree has been discarded
// 1-1) Setup mesh interface automatically
SetMeshInterface(create.mIMesh);
bool Status = false;
AABBTree* LeafTree = null;
Internal Data;
// 2) Build a generic AABB Tree.
mSource = new AABBTree;
CHECKALLOC(mSource);
// 2-1) Setup a builder. Our primitives here are triangles from input mesh,
// so we use an AABBTreeOfTrianglesBuilder.....
{
AABBTreeOfTrianglesBuilder TB;
TB.mIMesh = create.mIMesh;
TB.mNbPrimitives = create.mIMesh->GetNbTriangles();
TB.mSettings = create.mSettings;
TB.mSettings.mLimit = 16; // ### Hardcoded, but maybe we could let the user choose 8 / 16 / 32 ...
if(!mSource->Build(&TB)) goto FreeAndExit;
}
// 2-2) Here's the trick : create *another* AABB tree using the leaves of the first one (which are boxes, this time)
struct Local
{
// A callback to count leaf nodes
static bool CountLeaves(const AABBTreeNode* current, udword depth, void* user_data)
{
if(current->IsLeaf())
{
Internal* Data = (Internal*)user_data;
Data->mNbLeaves++;
}
return true;
}
// A callback to setup leaf nodes in our internal structures
static bool SetupLeafData(const AABBTreeNode* current, udword depth, void* user_data)
{
if(current->IsLeaf())
{
Internal* Data = (Internal*)user_data;
// Get current leaf's box
Data->mLeaves[Data->mNbLeaves] = *current->GetAABB();
// Setup leaf data
udword Index = udword((size_t(current->GetPrimitives()) - size_t(Data->mBase)) / sizeof(udword));
Data->mTriangles[Data->mNbLeaves].SetData(current->GetNbPrimitives(), Index);
Data->mNbLeaves++;
}
return true;
}
};
// Walk the tree & count number of leaves
Data.mNbLeaves = 0;
mSource->Walk(Local::CountLeaves, &Data);
mNbLeaves = Data.mNbLeaves; // Keep track of it
// Special case for 1-leaf meshes
if(mNbLeaves==1)
{
mModelCode |= OPC_SINGLE_NODE;
Status = true;
goto FreeAndExit;
}
// Allocate our structures
Data.mLeaves = new AABB[Data.mNbLeaves]; CHECKALLOC(Data.mLeaves);
mTriangles = new LeafTriangles[Data.mNbLeaves]; CHECKALLOC(mTriangles);
// Walk the tree again & setup leaf data
Data.mTriangles = mTriangles;
Data.mBase = mSource->GetIndices();
Data.mNbLeaves = 0; // Reset for incoming walk
mSource->Walk(Local::SetupLeafData, &Data);
// Handle source indices
{
bool MustKeepIndices = true;
if(create.mCanRemap)
{
// We try to get rid of source indices (saving more ram!) by reorganizing triangle arrays...
// Remap can fail when we use callbacks => keep track of indices in that case (it still
// works, only using more memory)
if(create.mIMesh->RemapClient(mSource->GetNbPrimitives(), mSource->GetIndices()))
{
MustKeepIndices = false;
}
}
if(MustKeepIndices)
{
// Keep track of source indices (from vanilla tree)
mNbPrimitives = mSource->GetNbPrimitives();
mIndices = new udword[mNbPrimitives];
CopyMemory(mIndices, mSource->GetIndices(), mNbPrimitives*sizeof(udword));
}
}
// Now, create our optimized tree using previous leaf nodes
LeafTree = new AABBTree;
CHECKALLOC(LeafTree);
{
AABBTreeOfAABBsBuilder TB; // Now using boxes !
TB.mSettings = create.mSettings;
TB.mSettings.mLimit = 1; // We now want a complete tree so that we can "optimize" it
TB.mNbPrimitives = Data.mNbLeaves;
TB.mAABBArray = Data.mLeaves;
if(!LeafTree->Build(&TB)) goto FreeAndExit;
}
// 3) Create an optimized tree according to user-settings
if(!CreateTree(create.mNoLeaf, create.mQuantized)) goto FreeAndExit;
// 3-2) Create optimized tree
if(!mTree->Build(LeafTree)) goto FreeAndExit;
// Finally ok...
Status = true;
FreeAndExit: // Allow me this one...
DELETESINGLE(LeafTree);
// 3-3) Delete generic tree if needed
if(!create.mKeepOriginal) DELETESINGLE(mSource);
return Status;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Gets the number of bytes used by the tree.
* \return amount of bytes used
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
udword HybridModel::GetUsedBytes() const
{
udword UsedBytes = 0;
if(mTree) UsedBytes += mTree->GetUsedBytes();
if(mIndices) UsedBytes += mNbPrimitives * sizeof(udword); // mIndices
if(mTriangles) UsedBytes += mNbLeaves * sizeof(LeafTriangles); // mTriangles
return UsedBytes;
}
inline_ void ComputeMinMax(Point& min, Point& max, const VertexPointers& vp)
{
// Compute triangle's AABB = a leaf box
#ifdef OPC_USE_FCOMI // a 15% speedup on my machine, not much
min.x = FCMin3(vp.Vertex[0]->x, vp.Vertex[1]->x, vp.Vertex[2]->x);
max.x = FCMax3(vp.Vertex[0]->x, vp.Vertex[1]->x, vp.Vertex[2]->x);
min.y = FCMin3(vp.Vertex[0]->y, vp.Vertex[1]->y, vp.Vertex[2]->y);
max.y = FCMax3(vp.Vertex[0]->y, vp.Vertex[1]->y, vp.Vertex[2]->y);
min.z = FCMin3(vp.Vertex[0]->z, vp.Vertex[1]->z, vp.Vertex[2]->z);
max.z = FCMax3(vp.Vertex[0]->z, vp.Vertex[1]->z, vp.Vertex[2]->z);
#else
min = *vp.Vertex[0];
max = *vp.Vertex[0];
min.Min(*vp.Vertex[1]);
max.Max(*vp.Vertex[1]);
min.Min(*vp.Vertex[2]);
max.Max(*vp.Vertex[2]);
#endif
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Refits the collision model. This can be used to handle dynamic meshes. Usage is:
* 1. modify your mesh vertices (keep the topology constant!)
* 2. refit the tree (call this method)
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool HybridModel::Refit()
{
if(!mIMesh) return false;
if(!mTree) return false;
if(IsQuantized()) return false;
if(HasLeafNodes()) return false;
const LeafTriangles* LT = GetLeafTriangles();
const udword* Indices = GetIndices();
// Bottom-up update
VertexPointers VP;
Point Min,Max;
Point Min_,Max_;
udword Index = mTree->GetNbNodes();
AABBNoLeafNode* Nodes = (AABBNoLeafNode*)((AABBNoLeafTree*)mTree)->GetNodes();
while(Index--)
{
AABBNoLeafNode& Current = Nodes[Index];
if(Current.HasPosLeaf())
{
const LeafTriangles& CurrentLeaf = LT[Current.GetPosPrimitive()];
Min.SetPlusInfinity();
Max.SetMinusInfinity();
Point TmpMin, TmpMax;
// Each leaf box has a set of triangles
udword NbTris = CurrentLeaf.GetNbTriangles();
if(Indices)
{
const udword* T = &Indices[CurrentLeaf.GetTriangleIndex()];
// Loop through triangles and test each of them
while(NbTris--)
{
mIMesh->GetTriangle(VP, *T++);
ComputeMinMax(TmpMin, TmpMax, VP);
Min.Min(TmpMin);
Max.Max(TmpMax);
}
}
else
{
udword BaseIndex = CurrentLeaf.GetTriangleIndex();
// Loop through triangles and test each of them
while(NbTris--)
{
mIMesh->GetTriangle(VP, BaseIndex++);
ComputeMinMax(TmpMin, TmpMax, VP);
Min.Min(TmpMin);
Max.Max(TmpMax);
}
}
}
else
{
const CollisionAABB& CurrentBox = Current.GetPos()->mAABB;
CurrentBox.GetMin(Min);
CurrentBox.GetMax(Max);
}
if(Current.HasNegLeaf())
{
const LeafTriangles& CurrentLeaf = LT[Current.GetNegPrimitive()];
Min_.SetPlusInfinity();
Max_.SetMinusInfinity();
Point TmpMin, TmpMax;
// Each leaf box has a set of triangles
udword NbTris = CurrentLeaf.GetNbTriangles();
if(Indices)
{
const udword* T = &Indices[CurrentLeaf.GetTriangleIndex()];
// Loop through triangles and test each of them
while(NbTris--)
{
mIMesh->GetTriangle(VP, *T++);
ComputeMinMax(TmpMin, TmpMax, VP);
Min_.Min(TmpMin);
Max_.Max(TmpMax);
}
}
else
{
udword BaseIndex = CurrentLeaf.GetTriangleIndex();
// Loop through triangles and test each of them
while(NbTris--)
{
mIMesh->GetTriangle(VP, BaseIndex++);
ComputeMinMax(TmpMin, TmpMax, VP);
Min_.Min(TmpMin);
Max_.Max(TmpMax);
}
}
}
else
{
const CollisionAABB& CurrentBox = Current.GetNeg()->mAABB;
CurrentBox.GetMin(Min_);
CurrentBox.GetMax(Max_);
}
#ifdef OPC_USE_FCOMI
Min.x = FCMin2(Min.x, Min_.x);
Max.x = FCMax2(Max.x, Max_.x);
Min.y = FCMin2(Min.y, Min_.y);
Max.y = FCMax2(Max.y, Max_.y);
Min.z = FCMin2(Min.z, Min_.z);
Max.z = FCMax2(Max.z, Max_.z);
#else
Min.Min(Min_);
Max.Max(Max_);
#endif
Current.mAABB.SetMinMax(Min, Max);
}
return true;
}

106
ode/OPCODE/OPC_HybridModel.h Normal file
View File

@ -0,0 +1,106 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for hybrid models.
* \file OPC_HybridModel.h
* \author Pierre Terdiman
* \date May, 18, 2003
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __OPC_HYBRIDMODEL_H__
#define __OPC_HYBRIDMODEL_H__
//! Leaf descriptor
struct LeafTriangles
{
udword Data; //!< Packed data
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Gets number of triangles in the leaf.
* \return number of triangles N, with 0 < N <= 16
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ udword GetNbTriangles() const { return (Data & 15)+1; }
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Gets triangle index for this leaf. Indexed model's array of indices retrieved with HybridModel::GetIndices()
* \return triangle index
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ udword GetTriangleIndex() const { return Data>>4; }
inline_ void SetData(udword nb, udword index) { ASSERT(nb>0 && nb<=16); nb--; Data = (index<<4)|(nb&15); }
};
class OPCODE_API HybridModel : public BaseModel
{
public:
// Constructor/Destructor
HybridModel();
virtual ~HybridModel();
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Builds a collision model.
* \param create [in] model creation structure
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
override(BaseModel) bool Build(const OPCODECREATE& create);
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Gets the number of bytes used by the tree.
* \return amount of bytes used
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
override(BaseModel) udword GetUsedBytes() const;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Refits the collision model. This can be used to handle dynamic meshes. Usage is:
* 1. modify your mesh vertices (keep the topology constant!)
* 2. refit the tree (call this method)
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
override(BaseModel) bool Refit();
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Gets array of triangles.
* \return array of triangles
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ const LeafTriangles* GetLeafTriangles() const { return mTriangles; }
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Gets array of indices.
* \return array of indices
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ const udword* GetIndices() const { return mIndices; }
private:
udword mNbLeaves; //!< Number of leaf nodes in the model
LeafTriangles* mTriangles; //!< Array of mNbLeaves leaf descriptors
udword mNbPrimitives; //!< Number of primitives in the model
udword* mIndices; //!< Array of primitive indices
// Internal methods
void Release();
};
#endif // __OPC_HYBRIDMODEL_H__

70
ode/OPCODE/OPC_IceHook.h Normal file
View File

@ -0,0 +1,70 @@
// Should be included by Opcode.h if needed
#define ICE_DONT_CHECK_COMPILER_OPTIONS
// From Windows...
typedef int BOOL;
#ifndef FALSE
#define FALSE 0
#endif
#ifndef TRUE
#define TRUE 1
#endif
#include <stdio.h>
#include <stdlib.h>
#include <assert.h>
#include <string.h>
#include <float.h>
#include <math.h>
#ifndef ASSERT
#define ASSERT(exp) {}
#endif
#define ICE_COMPILE_TIME_ASSERT(exp) extern char ICE_Dummy[ (exp) ? 1 : -1 ]
#define Log {}
#define SetIceError(a,b) false
#define EC_OUTOFMEMORY "Out of memory"
#include "Ice/IcePreprocessor.h"
#undef ICECORE_API
#define ICECORE_API OPCODE_API
#include "Ice/IceTypes.h"
#include "Ice/IceFPU.h"
#include "Ice/IceMemoryMacros.h"
namespace IceCore
{
#include "Ice/IceUtils.h"
#include "Ice/IceContainer.h"
#include "Ice/IcePairs.h"
#include "Ice/IceRevisitedRadix.h"
#include "Ice/IceRandom.h"
}
using namespace IceCore;
#define ICEMATHS_API OPCODE_API
namespace IceMaths
{
#include "Ice/IceAxes.h"
#include "Ice/IcePoint.h"
#include "Ice/IceHPoint.h"
#include "Ice/IceMatrix3x3.h"
#include "Ice/IceMatrix4x4.h"
#include "Ice/IcePlane.h"
#include "Ice/IceRay.h"
#include "Ice/IceIndexedTriangle.h"
#include "Ice/IceTriangle.h"
#include "Ice/IceTriList.h"
#include "Ice/IceAABB.h"
#include "Ice/IceOBB.h"
#include "Ice/IceBoundingSphere.h"
#include "Ice/IceSegment.h"
#include "Ice/IceLSS.h"
}
using namespace IceMaths;

523
ode/OPCODE/OPC_LSSAABBOverlap.h Normal file
View File

@ -0,0 +1,523 @@
// Following code from Magic-Software (http://www.magic-software.com/)
// A bit modified for Opcode
inline_ float OPC_PointAABBSqrDist(const Point& point, const Point& center, const Point& extents)
{
// Compute coordinates of point in box coordinate system
Point Closest = point - center;
float SqrDistance = 0.0f;
if(Closest.x < -extents.x)
{
float Delta = Closest.x + extents.x;
SqrDistance += Delta*Delta;
}
else if(Closest.x > extents.x)
{
float Delta = Closest.x - extents.x;
SqrDistance += Delta*Delta;
}
if(Closest.y < -extents.y)
{
float Delta = Closest.y + extents.y;
SqrDistance += Delta*Delta;
}
else if(Closest.y > extents.y)
{
float Delta = Closest.y - extents.y;
SqrDistance += Delta*Delta;
}
if(Closest.z < -extents.z)
{
float Delta = Closest.z + extents.z;
SqrDistance += Delta*Delta;
}
else if(Closest.z > extents.z)
{
float Delta = Closest.z - extents.z;
SqrDistance += Delta*Delta;
}
return SqrDistance;
}
static void Face(int i0, int i1, int i2, Point& rkPnt, const Point& rkDir, const Point& extents, const Point& rkPmE, float* pfLParam, float& rfSqrDistance)
{
Point kPpE;
float fLSqr, fInv, fTmp, fParam, fT, fDelta;
kPpE[i1] = rkPnt[i1] + extents[i1];
kPpE[i2] = rkPnt[i2] + extents[i2];
if(rkDir[i0]*kPpE[i1] >= rkDir[i1]*rkPmE[i0])
{
if(rkDir[i0]*kPpE[i2] >= rkDir[i2]*rkPmE[i0])
{
// v[i1] >= -e[i1], v[i2] >= -e[i2] (distance = 0)
if(pfLParam)
{
rkPnt[i0] = extents[i0];
fInv = 1.0f/rkDir[i0];
rkPnt[i1] -= rkDir[i1]*rkPmE[i0]*fInv;
rkPnt[i2] -= rkDir[i2]*rkPmE[i0]*fInv;
*pfLParam = -rkPmE[i0]*fInv;
}
}
else
{
// v[i1] >= -e[i1], v[i2] < -e[i2]
fLSqr = rkDir[i0]*rkDir[i0] + rkDir[i2]*rkDir[i2];
fTmp = fLSqr*kPpE[i1] - rkDir[i1]*(rkDir[i0]*rkPmE[i0] + rkDir[i2]*kPpE[i2]);
if(fTmp <= 2.0f*fLSqr*extents[i1])
{
fT = fTmp/fLSqr;
fLSqr += rkDir[i1]*rkDir[i1];
fTmp = kPpE[i1] - fT;
fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*fTmp + rkDir[i2]*kPpE[i2];
fParam = -fDelta/fLSqr;
rfSqrDistance += rkPmE[i0]*rkPmE[i0] + fTmp*fTmp + kPpE[i2]*kPpE[i2] + fDelta*fParam;
if(pfLParam)
{
*pfLParam = fParam;
rkPnt[i0] = extents[i0];
rkPnt[i1] = fT - extents[i1];
rkPnt[i2] = -extents[i2];
}
}
else
{
fLSqr += rkDir[i1]*rkDir[i1];
fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*rkPmE[i1] + rkDir[i2]*kPpE[i2];
fParam = -fDelta/fLSqr;
rfSqrDistance += rkPmE[i0]*rkPmE[i0] + rkPmE[i1]*rkPmE[i1] + kPpE[i2]*kPpE[i2] + fDelta*fParam;
if(pfLParam)
{
*pfLParam = fParam;
rkPnt[i0] = extents[i0];
rkPnt[i1] = extents[i1];
rkPnt[i2] = -extents[i2];
}
}
}
}
else
{
if ( rkDir[i0]*kPpE[i2] >= rkDir[i2]*rkPmE[i0] )
{
// v[i1] < -e[i1], v[i2] >= -e[i2]
fLSqr = rkDir[i0]*rkDir[i0] + rkDir[i1]*rkDir[i1];
fTmp = fLSqr*kPpE[i2] - rkDir[i2]*(rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1]);
if(fTmp <= 2.0f*fLSqr*extents[i2])
{
fT = fTmp/fLSqr;
fLSqr += rkDir[i2]*rkDir[i2];
fTmp = kPpE[i2] - fT;
fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] + rkDir[i2]*fTmp;
fParam = -fDelta/fLSqr;
rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] + fTmp*fTmp + fDelta*fParam;
if(pfLParam)
{
*pfLParam = fParam;
rkPnt[i0] = extents[i0];
rkPnt[i1] = -extents[i1];
rkPnt[i2] = fT - extents[i2];
}
}
else
{
fLSqr += rkDir[i2]*rkDir[i2];
fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] + rkDir[i2]*rkPmE[i2];
fParam = -fDelta/fLSqr;
rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] + rkPmE[i2]*rkPmE[i2] + fDelta*fParam;
if(pfLParam)
{
*pfLParam = fParam;
rkPnt[i0] = extents[i0];
rkPnt[i1] = -extents[i1];
rkPnt[i2] = extents[i2];
}
}
}
else
{
// v[i1] < -e[i1], v[i2] < -e[i2]
fLSqr = rkDir[i0]*rkDir[i0]+rkDir[i2]*rkDir[i2];
fTmp = fLSqr*kPpE[i1] - rkDir[i1]*(rkDir[i0]*rkPmE[i0] + rkDir[i2]*kPpE[i2]);
if(fTmp >= 0.0f)
{
// v[i1]-edge is closest
if ( fTmp <= 2.0f*fLSqr*extents[i1] )
{
fT = fTmp/fLSqr;
fLSqr += rkDir[i1]*rkDir[i1];
fTmp = kPpE[i1] - fT;
fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*fTmp + rkDir[i2]*kPpE[i2];
fParam = -fDelta/fLSqr;
rfSqrDistance += rkPmE[i0]*rkPmE[i0] + fTmp*fTmp + kPpE[i2]*kPpE[i2] + fDelta*fParam;
if(pfLParam)
{
*pfLParam = fParam;
rkPnt[i0] = extents[i0];
rkPnt[i1] = fT - extents[i1];
rkPnt[i2] = -extents[i2];
}
}
else
{
fLSqr += rkDir[i1]*rkDir[i1];
fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*rkPmE[i1] + rkDir[i2]*kPpE[i2];
fParam = -fDelta/fLSqr;
rfSqrDistance += rkPmE[i0]*rkPmE[i0] + rkPmE[i1]*rkPmE[i1] + kPpE[i2]*kPpE[i2] + fDelta*fParam;
if(pfLParam)
{
*pfLParam = fParam;
rkPnt[i0] = extents[i0];
rkPnt[i1] = extents[i1];
rkPnt[i2] = -extents[i2];
}
}
return;
}
fLSqr = rkDir[i0]*rkDir[i0] + rkDir[i1]*rkDir[i1];
fTmp = fLSqr*kPpE[i2] - rkDir[i2]*(rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1]);
if(fTmp >= 0.0f)
{
// v[i2]-edge is closest
if(fTmp <= 2.0f*fLSqr*extents[i2])
{
fT = fTmp/fLSqr;
fLSqr += rkDir[i2]*rkDir[i2];
fTmp = kPpE[i2] - fT;
fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] + rkDir[i2]*fTmp;
fParam = -fDelta/fLSqr;
rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] + fTmp*fTmp + fDelta*fParam;
if(pfLParam)
{
*pfLParam = fParam;
rkPnt[i0] = extents[i0];
rkPnt[i1] = -extents[i1];
rkPnt[i2] = fT - extents[i2];
}
}
else
{
fLSqr += rkDir[i2]*rkDir[i2];
fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] + rkDir[i2]*rkPmE[i2];
fParam = -fDelta/fLSqr;
rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] + rkPmE[i2]*rkPmE[i2] + fDelta*fParam;
if(pfLParam)
{
*pfLParam = fParam;
rkPnt[i0] = extents[i0];
rkPnt[i1] = -extents[i1];
rkPnt[i2] = extents[i2];
}
}
return;
}
// (v[i1],v[i2])-corner is closest
fLSqr += rkDir[i2]*rkDir[i2];
fDelta = rkDir[i0]*rkPmE[i0] + rkDir[i1]*kPpE[i1] + rkDir[i2]*kPpE[i2];
fParam = -fDelta/fLSqr;
rfSqrDistance += rkPmE[i0]*rkPmE[i0] + kPpE[i1]*kPpE[i1] + kPpE[i2]*kPpE[i2] + fDelta*fParam;
if(pfLParam)
{
*pfLParam = fParam;
rkPnt[i0] = extents[i0];
rkPnt[i1] = -extents[i1];
rkPnt[i2] = -extents[i2];
}
}
}
}
static void CaseNoZeros(Point& rkPnt, const Point& rkDir, const Point& extents, float* pfLParam, float& rfSqrDistance)
{
Point kPmE(rkPnt.x - extents.x, rkPnt.y - extents.y, rkPnt.z - extents.z);
float fProdDxPy, fProdDyPx, fProdDzPx, fProdDxPz, fProdDzPy, fProdDyPz;
fProdDxPy = rkDir.x*kPmE.y;
fProdDyPx = rkDir.y*kPmE.x;
if(fProdDyPx >= fProdDxPy)
{
fProdDzPx = rkDir.z*kPmE.x;
fProdDxPz = rkDir.x*kPmE.z;
if(fProdDzPx >= fProdDxPz)
{
// line intersects x = e0
Face(0, 1, 2, rkPnt, rkDir, extents, kPmE, pfLParam, rfSqrDistance);
}
else
{
// line intersects z = e2
Face(2, 0, 1, rkPnt, rkDir, extents, kPmE, pfLParam, rfSqrDistance);
}
}
else
{
fProdDzPy = rkDir.z*kPmE.y;
fProdDyPz = rkDir.y*kPmE.z;
if(fProdDzPy >= fProdDyPz)
{
// line intersects y = e1
Face(1, 2, 0, rkPnt, rkDir, extents, kPmE, pfLParam, rfSqrDistance);
}
else
{
// line intersects z = e2
Face(2, 0, 1, rkPnt, rkDir, extents, kPmE, pfLParam, rfSqrDistance);
}
}
}
static void Case0(int i0, int i1, int i2, Point& rkPnt, const Point& rkDir, const Point& extents, float* pfLParam, float& rfSqrDistance)
{
float fPmE0 = rkPnt[i0] - extents[i0];
float fPmE1 = rkPnt[i1] - extents[i1];
float fProd0 = rkDir[i1]*fPmE0;
float fProd1 = rkDir[i0]*fPmE1;
float fDelta, fInvLSqr, fInv;
if(fProd0 >= fProd1)
{
// line intersects P[i0] = e[i0]
rkPnt[i0] = extents[i0];
float fPpE1 = rkPnt[i1] + extents[i1];
fDelta = fProd0 - rkDir[i0]*fPpE1;
if(fDelta >= 0.0f)
{
fInvLSqr = 1.0f/(rkDir[i0]*rkDir[i0] + rkDir[i1]*rkDir[i1]);
rfSqrDistance += fDelta*fDelta*fInvLSqr;
if(pfLParam)
{
rkPnt[i1] = -extents[i1];
*pfLParam = -(rkDir[i0]*fPmE0+rkDir[i1]*fPpE1)*fInvLSqr;
}
}
else
{
if(pfLParam)
{
fInv = 1.0f/rkDir[i0];
rkPnt[i1] -= fProd0*fInv;
*pfLParam = -fPmE0*fInv;
}
}
}
else
{
// line intersects P[i1] = e[i1]
rkPnt[i1] = extents[i1];
float fPpE0 = rkPnt[i0] + extents[i0];
fDelta = fProd1 - rkDir[i1]*fPpE0;
if(fDelta >= 0.0f)
{
fInvLSqr = 1.0f/(rkDir[i0]*rkDir[i0] + rkDir[i1]*rkDir[i1]);
rfSqrDistance += fDelta*fDelta*fInvLSqr;
if(pfLParam)
{
rkPnt[i0] = -extents[i0];
*pfLParam = -(rkDir[i0]*fPpE0+rkDir[i1]*fPmE1)*fInvLSqr;
}
}
else
{
if(pfLParam)
{
fInv = 1.0f/rkDir[i1];
rkPnt[i0] -= fProd1*fInv;
*pfLParam = -fPmE1*fInv;
}
}
}
if(rkPnt[i2] < -extents[i2])
{
fDelta = rkPnt[i2] + extents[i2];
rfSqrDistance += fDelta*fDelta;
rkPnt[i2] = -extents[i2];
}
else if ( rkPnt[i2] > extents[i2] )
{
fDelta = rkPnt[i2] - extents[i2];
rfSqrDistance += fDelta*fDelta;
rkPnt[i2] = extents[i2];
}
}
static void Case00(int i0, int i1, int i2, Point& rkPnt, const Point& rkDir, const Point& extents, float* pfLParam, float& rfSqrDistance)
{
float fDelta;
if(pfLParam)
*pfLParam = (extents[i0] - rkPnt[i0])/rkDir[i0];
rkPnt[i0] = extents[i0];
if(rkPnt[i1] < -extents[i1])
{
fDelta = rkPnt[i1] + extents[i1];
rfSqrDistance += fDelta*fDelta;
rkPnt[i1] = -extents[i1];
}
else if(rkPnt[i1] > extents[i1])
{
fDelta = rkPnt[i1] - extents[i1];
rfSqrDistance += fDelta*fDelta;
rkPnt[i1] = extents[i1];
}
if(rkPnt[i2] < -extents[i2])
{
fDelta = rkPnt[i2] + extents[i2];
rfSqrDistance += fDelta*fDelta;
rkPnt[i1] = -extents[i2];
}
else if(rkPnt[i2] > extents[i2])
{
fDelta = rkPnt[i2] - extents[i2];
rfSqrDistance += fDelta*fDelta;
rkPnt[i2] = extents[i2];
}
}
static void Case000(Point& rkPnt, const Point& extents, float& rfSqrDistance)
{
float fDelta;
if(rkPnt.x < -extents.x)
{
fDelta = rkPnt.x + extents.x;
rfSqrDistance += fDelta*fDelta;
rkPnt.x = -extents.x;
}
else if(rkPnt.x > extents.x)
{
fDelta = rkPnt.x - extents.x;
rfSqrDistance += fDelta*fDelta;
rkPnt.x = extents.x;
}
if(rkPnt.y < -extents.y)
{
fDelta = rkPnt.y + extents.y;
rfSqrDistance += fDelta*fDelta;
rkPnt.y = -extents.y;
}
else if(rkPnt.y > extents.y)
{
fDelta = rkPnt.y - extents.y;
rfSqrDistance += fDelta*fDelta;
rkPnt.y = extents.y;
}
if(rkPnt.z < -extents.z)
{
fDelta = rkPnt.z + extents.z;
rfSqrDistance += fDelta*fDelta;
rkPnt.z = -extents.z;
}
else if(rkPnt.z > extents.z)
{
fDelta = rkPnt.z - extents.z;
rfSqrDistance += fDelta*fDelta;
rkPnt.z = extents.z;
}
}
static float SqrDistance(const Ray& rkLine, const Point& center, const Point& extents, float* pfLParam)
{
// compute coordinates of line in box coordinate system
Point kDiff = rkLine.mOrig - center;
Point kPnt = kDiff;
Point kDir = rkLine.mDir;
// Apply reflections so that direction vector has nonnegative components.
bool bReflect[3];
for(int i=0;i<3;i++)
{
if(kDir[i]<0.0f)
{
kPnt[i] = -kPnt[i];
kDir[i] = -kDir[i];
bReflect[i] = true;
}
else
{
bReflect[i] = false;
}
}
float fSqrDistance = 0.0f;
if(kDir.x>0.0f)
{
if(kDir.y>0.0f)
{
if(kDir.z>0.0f) CaseNoZeros(kPnt, kDir, extents, pfLParam, fSqrDistance); // (+,+,+)
else Case0(0, 1, 2, kPnt, kDir, extents, pfLParam, fSqrDistance); // (+,+,0)
}
else
{
if(kDir.z>0.0f) Case0(0, 2, 1, kPnt, kDir, extents, pfLParam, fSqrDistance); // (+,0,+)
else Case00(0, 1, 2, kPnt, kDir, extents, pfLParam, fSqrDistance); // (+,0,0)
}
}
else
{
if(kDir.y>0.0f)
{
if(kDir.z>0.0f) Case0(1, 2, 0, kPnt, kDir, extents, pfLParam, fSqrDistance); // (0,+,+)
else Case00(1, 0, 2, kPnt, kDir, extents, pfLParam, fSqrDistance); // (0,+,0)
}
else
{
if(kDir.z>0.0f) Case00(2, 0, 1, kPnt, kDir, extents, pfLParam, fSqrDistance); // (0,0,+)
else
{
Case000(kPnt, extents, fSqrDistance); // (0,0,0)
if(pfLParam) *pfLParam = 0.0f;
}
}
}
return fSqrDistance;
}
inline_ float OPC_SegmentOBBSqrDist(const Segment& segment, const Point& c0, const Point& e0)
{
float fLP;
float fSqrDistance = SqrDistance(Ray(segment.GetOrigin(), segment.ComputeDirection()), c0, e0, &fLP);
if(fLP>=0.0f)
{
if(fLP<=1.0f) return fSqrDistance;
else return OPC_PointAABBSqrDist(segment.mP1, c0, e0);
}
else return OPC_PointAABBSqrDist(segment.mP0, c0, e0);
}
inline_ BOOL LSSCollider::LSSAABBOverlap(const Point& center, const Point& extents)
{
// Stats
mNbVolumeBVTests++;
float s2 = OPC_SegmentOBBSqrDist(mSeg, center, extents);
if(s2<mRadius2) return TRUE;
return FALSE;
}

725
ode/OPCODE/OPC_LSSCollider.cpp Normal file
View File

@ -0,0 +1,725 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for an LSS collider.
* \file OPC_LSSCollider.cpp
* \author Pierre Terdiman
* \date December, 28, 2002
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains a lss-vs-tree collider.
*
* \class LSSCollider
* \author Pierre Terdiman
* \version 1.3
* \date December, 28, 2002
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Precompiled Header
#include "Stdafx.h"
using namespace Opcode;
#include "OPC_LSSAABBOverlap.h"
#include "OPC_LSSTriOverlap.h"
#define SET_CONTACT(prim_index, flag) \
/* Set contact status */ \
mFlags |= flag; \
mTouchedPrimitives->Add(udword(prim_index));
//! LSS-triangle overlap test
#define LSS_PRIM(prim_index, flag) \
/* Request vertices from the app */ \
VertexPointers VP; mIMesh->GetTriangle(VP, prim_index); \
\
/* Perform LSS-tri overlap test */ \
if(LSSTriOverlap(*VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2])) \
{ \
SET_CONTACT(prim_index, flag) \
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Constructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
LSSCollider::LSSCollider()
{
// mCenter.Zero();
// mRadius2 = 0.0f;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Destructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
LSSCollider::~LSSCollider()
{
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Generic collision query for generic OPCODE models. After the call, access the results:
* - with GetContactStatus()
* - with GetNbTouchedPrimitives()
* - with GetTouchedPrimitives()
*
* \param cache [in/out] an lss cache
* \param lss [in] collision lss in local space
* \param model [in] Opcode model to collide with
* \param worldl [in] lss world matrix, or null
* \param worldm [in] model's world matrix, or null
* \return true if success
* \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool LSSCollider::Collide(LSSCache& cache, const LSS& lss, const Model& model, const Matrix4x4* worldl, const Matrix4x4* worldm)
{
// Checkings
if(!Setup(&model)) return false;
// Init collision query
if(InitQuery(cache, lss, worldl, worldm)) return true;
if(!model.HasLeafNodes())
{
if(model.IsQuantized())
{
const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree();
// Setup dequantization coeffs
mCenterCoeff = Tree->mCenterCoeff;
mExtentsCoeff = Tree->mExtentsCoeff;
// Perform collision query
if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes());
else _Collide(Tree->GetNodes());
}
else
{
const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree();
// Perform collision query
if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes());
else _Collide(Tree->GetNodes());
}
}
else
{
if(model.IsQuantized())
{
const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree();
// Setup dequantization coeffs
mCenterCoeff = Tree->mCenterCoeff;
mExtentsCoeff = Tree->mExtentsCoeff;
// Perform collision query
if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes());
else _Collide(Tree->GetNodes());
}
else
{
const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree();
// Perform collision query
if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes());
else _Collide(Tree->GetNodes());
}
}
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Initializes a collision query :
* - reset stats & contact status
* - setup matrices
* - check temporal coherence
*
* \param cache [in/out] an lss cache
* \param lss [in] lss in local space
* \param worldl [in] lss world matrix, or null
* \param worldm [in] model's world matrix, or null
* \return TRUE if we can return immediately
* \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
BOOL LSSCollider::InitQuery(LSSCache& cache, const LSS& lss, const Matrix4x4* worldl, const Matrix4x4* worldm)
{
// 1) Call the base method
VolumeCollider::InitQuery();
// 2) Compute LSS in model space:
// - Precompute R^2
mRadius2 = lss.mRadius * lss.mRadius;
// - Compute segment
mSeg.mP0 = lss.mP0;
mSeg.mP1 = lss.mP1;
// -> to world space
if(worldl)
{
mSeg.mP0 *= *worldl;
mSeg.mP1 *= *worldl;
}
// -> to model space
if(worldm)
{
// Invert model matrix
Matrix4x4 InvWorldM;
InvertPRMatrix(InvWorldM, *worldm);
mSeg.mP0 *= InvWorldM;
mSeg.mP1 *= InvWorldM;
}
// 3) Setup destination pointer
mTouchedPrimitives = &cache.TouchedPrimitives;
// 4) Special case: 1-triangle meshes [Opcode 1.3]
if(mCurrentModel && mCurrentModel->HasSingleNode())
{
if(!SkipPrimitiveTests())
{
// We simply perform the BV-Prim overlap test each time. We assume single triangle has index 0.
mTouchedPrimitives->Reset();
// Perform overlap test between the unique triangle and the LSS (and set contact status if needed)
LSS_PRIM(udword(0), OPC_CONTACT)
// Return immediately regardless of status
return TRUE;
}
}
// 5) Check temporal coherence :
if(TemporalCoherenceEnabled())
{
// Here we use temporal coherence
// => check results from previous frame before performing the collision query
if(FirstContactEnabled())
{
// We're only interested in the first contact found => test the unique previously touched face
if(mTouchedPrimitives->GetNbEntries())
{
// Get index of previously touched face = the first entry in the array
udword PreviouslyTouchedFace = mTouchedPrimitives->GetEntry(0);
// Then reset the array:
// - if the overlap test below is successful, the index we'll get added back anyway
// - if it isn't, then the array should be reset anyway for the normal query
mTouchedPrimitives->Reset();
// Perform overlap test between the cached triangle and the LSS (and set contact status if needed)
LSS_PRIM(PreviouslyTouchedFace, OPC_TEMPORAL_CONTACT)
// Return immediately if possible
if(GetContactStatus()) return TRUE;
}
// else no face has been touched during previous query
// => we'll have to perform a normal query
}
else
{
// We're interested in all contacts =>test the new real LSS N(ew) against the previous fat LSS P(revious):
// ### rewrite this
LSS Test(mSeg, lss.mRadius); // in model space
LSS Previous(cache.Previous, sqrtf(cache.Previous.mRadius));
// if(cache.Previous.Contains(Test))
if(IsCacheValid(cache) && Previous.Contains(Test))
{
// - if N is included in P, return previous list
// => we simply leave the list (mTouchedFaces) unchanged
// Set contact status if needed
if(mTouchedPrimitives->GetNbEntries()) mFlags |= OPC_TEMPORAL_CONTACT;
// In any case we don't need to do a query
return TRUE;
}
else
{
// - else do the query using a fat N
// Reset cache since we'll about to perform a real query
mTouchedPrimitives->Reset();
// Make a fat sphere so that coherence will work for subsequent frames
mRadius2 *= cache.FatCoeff;
// mRadius2 = (lss.mRadius * cache.FatCoeff)*(lss.mRadius * cache.FatCoeff);
// Update cache with query data (signature for cached faces)
cache.Previous.mP0 = mSeg.mP0;
cache.Previous.mP1 = mSeg.mP1;
cache.Previous.mRadius = mRadius2;
}
}
}
else
{
// Here we don't use temporal coherence => do a normal query
mTouchedPrimitives->Reset();
}
return FALSE;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Collision query for vanilla AABB trees.
* \param cache [in/out] an lss cache
* \param lss [in] collision lss in world space
* \param tree [in] AABB tree
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool LSSCollider::Collide(LSSCache& cache, const LSS& lss, const AABBTree* tree)
{
// This is typically called for a scene tree, full of -AABBs-, not full of triangles.
// So we don't really have "primitives" to deal with. Hence it doesn't work with
// "FirstContact" + "TemporalCoherence".
ASSERT( !(FirstContactEnabled() && TemporalCoherenceEnabled()) );
// Checkings
if(!tree) return false;
// Init collision query
if(InitQuery(cache, lss)) return true;
// Perform collision query
_Collide(tree);
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Checks the LSS completely contains the box. In which case we can end the query sooner.
* \param bc [in] box center
* \param be [in] box extents
* \return true if the LSS contains the whole box
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ BOOL LSSCollider::LSSContainsBox(const Point& bc, const Point& be)
{
// Not implemented
return FALSE;
}
#define TEST_BOX_IN_LSS(center, extents) \
if(LSSContainsBox(center, extents)) \
{ \
/* Set contact status */ \
mFlags |= OPC_CONTACT; \
_Dump(node); \
return; \
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for normal AABB trees.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void LSSCollider::_Collide(const AABBCollisionNode* node)
{
// Perform LSS-AABB overlap test
if(!LSSAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents)) return;
TEST_BOX_IN_LSS(node->mAABB.mCenter, node->mAABB.mExtents)
if(node->IsLeaf())
{
LSS_PRIM(node->GetPrimitive(), OPC_CONTACT)
}
else
{
_Collide(node->GetPos());
if(ContactFound()) return;
_Collide(node->GetNeg());
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for normal AABB trees, without primitive tests.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void LSSCollider::_CollideNoPrimitiveTest(const AABBCollisionNode* node)
{
// Perform LSS-AABB overlap test
if(!LSSAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents)) return;
TEST_BOX_IN_LSS(node->mAABB.mCenter, node->mAABB.mExtents)
if(node->IsLeaf())
{
SET_CONTACT(node->GetPrimitive(), OPC_CONTACT)
}
else
{
_CollideNoPrimitiveTest(node->GetPos());
if(ContactFound()) return;
_CollideNoPrimitiveTest(node->GetNeg());
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for quantized AABB trees.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void LSSCollider::_Collide(const AABBQuantizedNode* node)
{
// Dequantize box
const QuantizedAABB& Box = node->mAABB;
const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
// Perform LSS-AABB overlap test
if(!LSSAABBOverlap(Center, Extents)) return;
TEST_BOX_IN_LSS(Center, Extents)
if(node->IsLeaf())
{
LSS_PRIM(node->GetPrimitive(), OPC_CONTACT)
}
else
{
_Collide(node->GetPos());
if(ContactFound()) return;
_Collide(node->GetNeg());
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for quantized AABB trees, without primitive tests.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void LSSCollider::_CollideNoPrimitiveTest(const AABBQuantizedNode* node)
{
// Dequantize box
const QuantizedAABB& Box = node->mAABB;
const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
// Perform LSS-AABB overlap test
if(!LSSAABBOverlap(Center, Extents)) return;
TEST_BOX_IN_LSS(Center, Extents)
if(node->IsLeaf())
{
SET_CONTACT(node->GetPrimitive(), OPC_CONTACT)
}
else
{
_CollideNoPrimitiveTest(node->GetPos());
if(ContactFound()) return;
_CollideNoPrimitiveTest(node->GetNeg());
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for no-leaf AABB trees.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void LSSCollider::_Collide(const AABBNoLeafNode* node)
{
// Perform LSS-AABB overlap test
if(!LSSAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents)) return;
TEST_BOX_IN_LSS(node->mAABB.mCenter, node->mAABB.mExtents)
if(node->HasPosLeaf()) { LSS_PRIM(node->GetPosPrimitive(), OPC_CONTACT) }
else _Collide(node->GetPos());
if(ContactFound()) return;
if(node->HasNegLeaf()) { LSS_PRIM(node->GetNegPrimitive(), OPC_CONTACT) }
else _Collide(node->GetNeg());
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for no-leaf AABB trees, without primitive tests.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void LSSCollider::_CollideNoPrimitiveTest(const AABBNoLeafNode* node)
{
// Perform LSS-AABB overlap test
if(!LSSAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents)) return;
TEST_BOX_IN_LSS(node->mAABB.mCenter, node->mAABB.mExtents)
if(node->HasPosLeaf()) { SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) }
else _CollideNoPrimitiveTest(node->GetPos());
if(ContactFound()) return;
if(node->HasNegLeaf()) { SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) }
else _CollideNoPrimitiveTest(node->GetNeg());
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for quantized no-leaf AABB trees.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void LSSCollider::_Collide(const AABBQuantizedNoLeafNode* node)
{
// Dequantize box
const QuantizedAABB& Box = node->mAABB;
const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
// Perform LSS-AABB overlap test
if(!LSSAABBOverlap(Center, Extents)) return;
TEST_BOX_IN_LSS(Center, Extents)
if(node->HasPosLeaf()) { LSS_PRIM(node->GetPosPrimitive(), OPC_CONTACT) }
else _Collide(node->GetPos());
if(ContactFound()) return;
if(node->HasNegLeaf()) { LSS_PRIM(node->GetNegPrimitive(), OPC_CONTACT) }
else _Collide(node->GetNeg());
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for quantized no-leaf AABB trees, without primitive tests.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void LSSCollider::_CollideNoPrimitiveTest(const AABBQuantizedNoLeafNode* node)
{
// Dequantize box
const QuantizedAABB& Box = node->mAABB;
const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
// Perform LSS-AABB overlap test
if(!LSSAABBOverlap(Center, Extents)) return;
TEST_BOX_IN_LSS(Center, Extents)
if(node->HasPosLeaf()) { SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) }
else _CollideNoPrimitiveTest(node->GetPos());
if(ContactFound()) return;
if(node->HasNegLeaf()) { SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) }
else _CollideNoPrimitiveTest(node->GetNeg());
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for vanilla AABB trees.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void LSSCollider::_Collide(const AABBTreeNode* node)
{
// Perform LSS-AABB overlap test
Point Center, Extents;
node->GetAABB()->GetCenter(Center);
node->GetAABB()->GetExtents(Extents);
if(!LSSAABBOverlap(Center, Extents)) return;
if(node->IsLeaf() || LSSContainsBox(Center, Extents))
{
mFlags |= OPC_CONTACT;
mTouchedPrimitives->Add(node->GetPrimitives(), node->GetNbPrimitives());
}
else
{
_Collide(node->GetPos());
_Collide(node->GetNeg());
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Constructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
HybridLSSCollider::HybridLSSCollider()
{
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Destructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
HybridLSSCollider::~HybridLSSCollider()
{
}
bool HybridLSSCollider::Collide(LSSCache& cache, const LSS& lss, const HybridModel& model, const Matrix4x4* worldl, const Matrix4x4* worldm)
{
// We don't want primitive tests here!
mFlags |= OPC_NO_PRIMITIVE_TESTS;
// Checkings
if(!Setup(&model)) return false;
// Init collision query
if(InitQuery(cache, lss, worldl, worldm)) return true;
// Special case for 1-leaf trees
if(mCurrentModel && mCurrentModel->HasSingleNode())
{
// Here we're supposed to perform a normal query, except our tree has a single node, i.e. just a few triangles
udword Nb = mIMesh->GetNbTriangles();
// Loop through all triangles
for(udword i=0;i<Nb;i++)
{
LSS_PRIM(i, OPC_CONTACT)
}
return true;
}
// Override destination array since we're only going to get leaf boxes here
mTouchedBoxes.Reset();
mTouchedPrimitives = &mTouchedBoxes;
// Now, do the actual query against leaf boxes
if(!model.HasLeafNodes())
{
if(model.IsQuantized())
{
const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree();
// Setup dequantization coeffs
mCenterCoeff = Tree->mCenterCoeff;
mExtentsCoeff = Tree->mExtentsCoeff;
// Perform collision query - we don't want primitive tests here!
_CollideNoPrimitiveTest(Tree->GetNodes());
}
else
{
const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree();
// Perform collision query - we don't want primitive tests here!
_CollideNoPrimitiveTest(Tree->GetNodes());
}
}
else
{
if(model.IsQuantized())
{
const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree();
// Setup dequantization coeffs
mCenterCoeff = Tree->mCenterCoeff;
mExtentsCoeff = Tree->mExtentsCoeff;
// Perform collision query - we don't want primitive tests here!
_CollideNoPrimitiveTest(Tree->GetNodes());
}
else
{
const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree();
// Perform collision query - we don't want primitive tests here!
_CollideNoPrimitiveTest(Tree->GetNodes());
}
}
// We only have a list of boxes so far
if(GetContactStatus())
{
// Reset contact status, since it currently only reflects collisions with leaf boxes
Collider::InitQuery();
// Change dest container so that we can use built-in overlap tests and get collided primitives
cache.TouchedPrimitives.Reset();
mTouchedPrimitives = &cache.TouchedPrimitives;
// Read touched leaf boxes
udword Nb = mTouchedBoxes.GetNbEntries();
const udword* Touched = mTouchedBoxes.GetEntries();
const LeafTriangles* LT = model.GetLeafTriangles();
const udword* Indices = model.GetIndices();
// Loop through touched leaves
while(Nb--)
{
const LeafTriangles& CurrentLeaf = LT[*Touched++];
// Each leaf box has a set of triangles
udword NbTris = CurrentLeaf.GetNbTriangles();
if(Indices)
{
const udword* T = &Indices[CurrentLeaf.GetTriangleIndex()];
// Loop through triangles and test each of them
while(NbTris--)
{
udword TriangleIndex = *T++;
LSS_PRIM(TriangleIndex, OPC_CONTACT)
}
}
else
{
udword BaseIndex = CurrentLeaf.GetTriangleIndex();
// Loop through triangles and test each of them
while(NbTris--)
{
udword TriangleIndex = BaseIndex++;
LSS_PRIM(TriangleIndex, OPC_CONTACT)
}
}
}
}
return true;
}

99
ode/OPCODE/OPC_LSSCollider.h Normal file
View File

@ -0,0 +1,99 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for an LSS collider.
* \file OPC_LSSCollider.h
* \author Pierre Terdiman
* \date December, 28, 2002
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __OPC_LSSCOLLIDER_H__
#define __OPC_LSSCOLLIDER_H__
struct OPCODE_API LSSCache : VolumeCache
{
LSSCache()
{
Previous.mP0 = Point(0.0f, 0.0f, 0.0f);
Previous.mP1 = Point(0.0f, 0.0f, 0.0f);
Previous.mRadius = 0.0f;
FatCoeff = 1.1f;
}
// Cached faces signature
LSS Previous; //!< LSS used when performing the query resulting in cached faces
// User settings
float FatCoeff; //!< mRadius2 multiplier used to create a fat LSS
};
class OPCODE_API LSSCollider : public VolumeCollider
{
public:
// Constructor / Destructor
LSSCollider();
virtual ~LSSCollider();
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Generic collision query for generic OPCODE models. After the call, access the results:
* - with GetContactStatus()
* - with GetNbTouchedPrimitives()
* - with GetTouchedPrimitives()
*
* \param cache [in/out] an lss cache
* \param lss [in] collision lss in local space
* \param model [in] Opcode model to collide with
* \param worldl [in] lss world matrix, or null
* \param worldm [in] model's world matrix, or null
* \return true if success
* \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool Collide(LSSCache& cache, const LSS& lss, const Model& model, const Matrix4x4* worldl=null, const Matrix4x4* worldm=null);
//
bool Collide(LSSCache& cache, const LSS& lss, const AABBTree* tree);
protected:
// LSS in model space
Segment mSeg; //!< Segment
float mRadius2; //!< LSS radius squared
// Internal methods
void _Collide(const AABBCollisionNode* node);
void _Collide(const AABBNoLeafNode* node);
void _Collide(const AABBQuantizedNode* node);
void _Collide(const AABBQuantizedNoLeafNode* node);
void _Collide(const AABBTreeNode* node);
void _CollideNoPrimitiveTest(const AABBCollisionNode* node);
void _CollideNoPrimitiveTest(const AABBNoLeafNode* node);
void _CollideNoPrimitiveTest(const AABBQuantizedNode* node);
void _CollideNoPrimitiveTest(const AABBQuantizedNoLeafNode* node);
// Overlap tests
inline_ BOOL LSSContainsBox(const Point& bc, const Point& be);
inline_ BOOL LSSAABBOverlap(const Point& center, const Point& extents);
inline_ BOOL LSSTriOverlap(const Point& vert0, const Point& vert1, const Point& vert2);
// Init methods
BOOL InitQuery(LSSCache& cache, const LSS& lss, const Matrix4x4* worldl=null, const Matrix4x4* worldm=null);
};
class OPCODE_API HybridLSSCollider : public LSSCollider
{
public:
// Constructor / Destructor
HybridLSSCollider();
virtual ~HybridLSSCollider();
bool Collide(LSSCache& cache, const LSS& lss, const HybridModel& model, const Matrix4x4* worldl=null, const Matrix4x4* worldm=null);
protected:
Container mTouchedBoxes;
};
#endif // __OPC_LSSCOLLIDER_H__

679
ode/OPCODE/OPC_LSSTriOverlap.h Normal file
View File

@ -0,0 +1,679 @@
// Following code from Magic-Software (http://www.magic-software.com/)
// A bit modified for Opcode
static const float gs_fTolerance = 1e-05f;
static float OPC_PointTriangleSqrDist(const Point& point, const Point& p0, const Point& p1, const Point& p2)
{
// Hook
Point TriEdge0 = p1 - p0;
Point TriEdge1 = p2 - p0;
Point kDiff = p0 - point;
float fA00 = TriEdge0.SquareMagnitude();
float fA01 = TriEdge0 | TriEdge1;
float fA11 = TriEdge1.SquareMagnitude();
float fB0 = kDiff | TriEdge0;
float fB1 = kDiff | TriEdge1;
float fC = kDiff.SquareMagnitude();
float fDet = fabsf(fA00*fA11 - fA01*fA01);
float fS = fA01*fB1-fA11*fB0;
float fT = fA01*fB0-fA00*fB1;
float fSqrDist;
if(fS + fT <= fDet)
{
if(fS < 0.0f)
{
if(fT < 0.0f) // region 4
{
if(fB0 < 0.0f)
{
if(-fB0 >= fA00) fSqrDist = fA00+2.0f*fB0+fC;
else fSqrDist = fB0*(-fB0/fA00)+fC;
}
else
{
if(fB1 >= 0.0f) fSqrDist = fC;
else if(-fB1 >= fA11) fSqrDist = fA11+2.0f*fB1+fC;
else fSqrDist = fB1*(-fB1/fA11)+fC;
}
}
else // region 3
{
if(fB1 >= 0.0f) fSqrDist = fC;
else if(-fB1 >= fA11) fSqrDist = fA11+2.0f*fB1+fC;
else fSqrDist = fB1*(-fB1/fA11)+fC;
}
}
else if(fT < 0.0f) // region 5
{
if(fB0 >= 0.0f) fSqrDist = fC;
else if(-fB0 >= fA00) fSqrDist = fA00+2.0f*fB0+fC;
else fSqrDist = fB0*(-fB0/fA00)+fC;
}
else // region 0
{
// minimum at interior point
if(fDet==0.0f)
{
fSqrDist = MAX_FLOAT;
}
else
{
float fInvDet = 1.0f/fDet;
fS *= fInvDet;
fT *= fInvDet;
fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) + fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC;
}
}
}
else
{
float fTmp0, fTmp1, fNumer, fDenom;
if(fS < 0.0f) // region 2
{
fTmp0 = fA01 + fB0;
fTmp1 = fA11 + fB1;
if(fTmp1 > fTmp0)
{
fNumer = fTmp1 - fTmp0;
fDenom = fA00-2.0f*fA01+fA11;
if(fNumer >= fDenom)
{
fSqrDist = fA00+2.0f*fB0+fC;
}
else
{
fS = fNumer/fDenom;
fT = 1.0f - fS;
fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) + fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC;
}
}
else
{
if(fTmp1 <= 0.0f) fSqrDist = fA11+2.0f*fB1+fC;
else if(fB1 >= 0.0f) fSqrDist = fC;
else fSqrDist = fB1*(-fB1/fA11)+fC;
}
}
else if(fT < 0.0f) // region 6
{
fTmp0 = fA01 + fB1;
fTmp1 = fA00 + fB0;
if(fTmp1 > fTmp0)
{
fNumer = fTmp1 - fTmp0;
fDenom = fA00-2.0f*fA01+fA11;
if(fNumer >= fDenom)
{
fSqrDist = fA11+2.0f*fB1+fC;
}
else
{
fT = fNumer/fDenom;
fS = 1.0f - fT;
fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) + fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC;
}
}
else
{
if(fTmp1 <= 0.0f) fSqrDist = fA00+2.0f*fB0+fC;
else if(fB0 >= 0.0f) fSqrDist = fC;
else fSqrDist = fB0*(-fB0/fA00)+fC;
}
}
else // region 1
{
fNumer = fA11 + fB1 - fA01 - fB0;
if(fNumer <= 0.0f)
{
fSqrDist = fA11+2.0f*fB1+fC;
}
else
{
fDenom = fA00-2.0f*fA01+fA11;
if(fNumer >= fDenom)
{
fSqrDist = fA00+2.0f*fB0+fC;
}
else
{
fS = fNumer/fDenom;
fT = 1.0f - fS;
fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) + fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC;
}
}
}
}
return fabsf(fSqrDist);
}
static float OPC_SegmentSegmentSqrDist(const Segment& rkSeg0, const Segment& rkSeg1)
{
// Hook
Point rkSeg0Direction = rkSeg0.ComputeDirection();
Point rkSeg1Direction = rkSeg1.ComputeDirection();
Point kDiff = rkSeg0.mP0 - rkSeg1.mP0;
float fA00 = rkSeg0Direction.SquareMagnitude();
float fA01 = -rkSeg0Direction.Dot(rkSeg1Direction);
float fA11 = rkSeg1Direction.SquareMagnitude();
float fB0 = kDiff.Dot(rkSeg0Direction);
float fC = kDiff.SquareMagnitude();
float fDet = fabsf(fA00*fA11-fA01*fA01);
float fB1, fS, fT, fSqrDist, fTmp;
if(fDet>=gs_fTolerance)
{
// line segments are not parallel
fB1 = -kDiff.Dot(rkSeg1Direction);
fS = fA01*fB1-fA11*fB0;
fT = fA01*fB0-fA00*fB1;
if(fS >= 0.0f)
{
if(fS <= fDet)
{
if(fT >= 0.0f)
{
if(fT <= fDet) // region 0 (interior)
{
// minimum at two interior points of 3D lines
float fInvDet = 1.0f/fDet;
fS *= fInvDet;
fT *= fInvDet;
fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) + fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC;
}
else // region 3 (side)
{
fTmp = fA01+fB0;
if(fTmp>=0.0f) fSqrDist = fA11+2.0f*fB1+fC;
else if(-fTmp>=fA00) fSqrDist = fA00+fA11+fC+2.0f*(fB1+fTmp);
else fSqrDist = fTmp*(-fTmp/fA00)+fA11+2.0f*fB1+fC;
}
}
else // region 7 (side)
{
if(fB0>=0.0f) fSqrDist = fC;
else if(-fB0>=fA00) fSqrDist = fA00+2.0f*fB0+fC;
else fSqrDist = fB0*(-fB0/fA00)+fC;
}
}
else
{
if ( fT >= 0.0 )
{
if ( fT <= fDet ) // region 1 (side)
{
fTmp = fA01+fB1;
if(fTmp>=0.0f) fSqrDist = fA00+2.0f*fB0+fC;
else if(-fTmp>=fA11) fSqrDist = fA00+fA11+fC+2.0f*(fB0+fTmp);
else fSqrDist = fTmp*(-fTmp/fA11)+fA00+2.0f*fB0+fC;
}
else // region 2 (corner)
{
fTmp = fA01+fB0;
if ( -fTmp <= fA00 )
{
if(fTmp>=0.0f) fSqrDist = fA11+2.0f*fB1+fC;
else fSqrDist = fTmp*(-fTmp/fA00)+fA11+2.0f*fB1+fC;
}
else
{
fTmp = fA01+fB1;
if(fTmp>=0.0f) fSqrDist = fA00+2.0f*fB0+fC;
else if(-fTmp>=fA11) fSqrDist = fA00+fA11+fC+2.0f*(fB0+fTmp);
else fSqrDist = fTmp*(-fTmp/fA11)+fA00+2.0f*fB0+fC;
}
}
}
else // region 8 (corner)
{
if ( -fB0 < fA00 )
{
if(fB0>=0.0f) fSqrDist = fC;
else fSqrDist = fB0*(-fB0/fA00)+fC;
}
else
{
fTmp = fA01+fB1;
if(fTmp>=0.0f) fSqrDist = fA00+2.0f*fB0+fC;
else if(-fTmp>=fA11) fSqrDist = fA00+fA11+fC+2.0f*(fB0+fTmp);
else fSqrDist = fTmp*(-fTmp/fA11)+fA00+2.0f*fB0+fC;
}
}
}
}
else
{
if ( fT >= 0.0f )
{
if ( fT <= fDet ) // region 5 (side)
{
if(fB1>=0.0f) fSqrDist = fC;
else if(-fB1>=fA11) fSqrDist = fA11+2.0f*fB1+fC;
else fSqrDist = fB1*(-fB1/fA11)+fC;
}
else // region 4 (corner)
{
fTmp = fA01+fB0;
if ( fTmp < 0.0f )
{
if(-fTmp>=fA00) fSqrDist = fA00+fA11+fC+2.0f*(fB1+fTmp);
else fSqrDist = fTmp*(-fTmp/fA00)+fA11+2.0f*fB1+fC;
}
else
{
if(fB1>=0.0f) fSqrDist = fC;
else if(-fB1>=fA11) fSqrDist = fA11+2.0f*fB1+fC;
else fSqrDist = fB1*(-fB1/fA11)+fC;
}
}
}
else // region 6 (corner)
{
if ( fB0 < 0.0f )
{
if(-fB0>=fA00) fSqrDist = fA00+2.0f*fB0+fC;
else fSqrDist = fB0*(-fB0/fA00)+fC;
}
else
{
if(fB1>=0.0f) fSqrDist = fC;
else if(-fB1>=fA11) fSqrDist = fA11+2.0f*fB1+fC;
else fSqrDist = fB1*(-fB1/fA11)+fC;
}
}
}
}
else
{
// line segments are parallel
if ( fA01 > 0.0f )
{
// direction vectors form an obtuse angle
if ( fB0 >= 0.0f )
{
fSqrDist = fC;
}
else if ( -fB0 <= fA00 )
{
fSqrDist = fB0*(-fB0/fA00)+fC;
}
else
{
fB1 = -kDiff.Dot(rkSeg1Direction);
fTmp = fA00+fB0;
if ( -fTmp >= fA01 )
{
fSqrDist = fA00+fA11+fC+2.0f*(fA01+fB0+fB1);
}
else
{
fT = -fTmp/fA01;
fSqrDist = fA00+2.0f*fB0+fC+fT*(fA11*fT+2.0f*(fA01+fB1));
}
}
}
else
{
// direction vectors form an acute angle
if ( -fB0 >= fA00 )
{
fSqrDist = fA00+2.0f*fB0+fC;
}
else if ( fB0 <= 0.0f )
{
fSqrDist = fB0*(-fB0/fA00)+fC;
}
else
{
fB1 = -kDiff.Dot(rkSeg1Direction);
if ( fB0 >= -fA01 )
{
fSqrDist = fA11+2.0f*fB1+fC;
}
else
{
fT = -fB0/fA01;
fSqrDist = fC+fT*(2.0f*fB1+fA11*fT);
}
}
}
}
return fabsf(fSqrDist);
}
inline_ float OPC_SegmentRaySqrDist(const Segment& rkSeg0, const Ray& rkSeg1)
{
return OPC_SegmentSegmentSqrDist(rkSeg0, Segment(rkSeg1.mOrig, rkSeg1.mOrig + rkSeg1.mDir));
}
static float OPC_SegmentTriangleSqrDist(const Segment& segment, const Point& p0, const Point& p1, const Point& p2)
{
// Hook
const Point TriEdge0 = p1 - p0;
const Point TriEdge1 = p2 - p0;
const Point& rkSegOrigin = segment.GetOrigin();
Point rkSegDirection = segment.ComputeDirection();
Point kDiff = p0 - rkSegOrigin;
float fA00 = rkSegDirection.SquareMagnitude();
float fA01 = -rkSegDirection.Dot(TriEdge0);
float fA02 = -rkSegDirection.Dot(TriEdge1);
float fA11 = TriEdge0.SquareMagnitude();
float fA12 = TriEdge0.Dot(TriEdge1);
float fA22 = TriEdge1.Dot(TriEdge1);
float fB0 = -kDiff.Dot(rkSegDirection);
float fB1 = kDiff.Dot(TriEdge0);
float fB2 = kDiff.Dot(TriEdge1);
float fCof00 = fA11*fA22-fA12*fA12;
float fCof01 = fA02*fA12-fA01*fA22;
float fCof02 = fA01*fA12-fA02*fA11;
float fDet = fA00*fCof00+fA01*fCof01+fA02*fCof02;
Ray kTriSeg;
Point kPt;
float fSqrDist, fSqrDist0;
if(fabsf(fDet)>=gs_fTolerance)
{
float fCof11 = fA00*fA22-fA02*fA02;
float fCof12 = fA02*fA01-fA00*fA12;
float fCof22 = fA00*fA11-fA01*fA01;
float fInvDet = 1.0f/fDet;
float fRhs0 = -fB0*fInvDet;
float fRhs1 = -fB1*fInvDet;
float fRhs2 = -fB2*fInvDet;
float fR = fCof00*fRhs0+fCof01*fRhs1+fCof02*fRhs2;
float fS = fCof01*fRhs0+fCof11*fRhs1+fCof12*fRhs2;
float fT = fCof02*fRhs0+fCof12*fRhs1+fCof22*fRhs2;
if ( fR < 0.0f )
{
if ( fS+fT <= 1.0f )
{
if ( fS < 0.0f )
{
if ( fT < 0.0f ) // region 4m
{
// min on face s=0 or t=0 or r=0
kTriSeg.mOrig = p0;
kTriSeg.mDir = TriEdge1;
fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
kTriSeg.mOrig = p0;
kTriSeg.mDir = TriEdge0;
fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg);
if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
fSqrDist0 = OPC_PointTriangleSqrDist(rkSegOrigin, p0, p1, p2);
if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
}
else // region 3m
{
// min on face s=0 or r=0
kTriSeg.mOrig = p0;
kTriSeg.mDir = TriEdge1;
fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
fSqrDist0 = OPC_PointTriangleSqrDist(rkSegOrigin, p0, p1, p2);
if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
}
}
else if ( fT < 0.0f ) // region 5m
{
// min on face t=0 or r=0
kTriSeg.mOrig = p0;
kTriSeg.mDir = TriEdge0;
fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
fSqrDist0 = OPC_PointTriangleSqrDist(rkSegOrigin, p0, p1, p2);
if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
}
else // region 0m
{
// min on face r=0
fSqrDist = OPC_PointTriangleSqrDist(rkSegOrigin, p0, p1, p2);
}
}
else
{
if ( fS < 0.0f ) // region 2m
{
// min on face s=0 or s+t=1 or r=0
kTriSeg.mOrig = p0;
kTriSeg.mDir = TriEdge1;
fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
kTriSeg.mOrig = p1;
kTriSeg.mDir = TriEdge1-TriEdge0;
fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg);
if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
fSqrDist0 = OPC_PointTriangleSqrDist(rkSegOrigin, p0, p1, p2);
if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
}
else if ( fT < 0.0f ) // region 6m
{
// min on face t=0 or s+t=1 or r=0
kTriSeg.mOrig = p0;
kTriSeg.mDir = TriEdge0;
fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
kTriSeg.mOrig = p1;
kTriSeg.mDir = TriEdge1-TriEdge0;
fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg);
if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
fSqrDist0 = OPC_PointTriangleSqrDist(rkSegOrigin, p0, p1, p2);
if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
}
else // region 1m
{
// min on face s+t=1 or r=0
kTriSeg.mOrig = p1;
kTriSeg.mDir = TriEdge1-TriEdge0;
fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
fSqrDist0 = OPC_PointTriangleSqrDist(rkSegOrigin, p0, p1, p2);
if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
}
}
}
else if ( fR <= 1.0f )
{
if ( fS+fT <= 1.0f )
{
if ( fS < 0.0f )
{
if ( fT < 0.0f ) // region 4
{
// min on face s=0 or t=0
kTriSeg.mOrig = p0;
kTriSeg.mDir = TriEdge1;
fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
kTriSeg.mOrig = p0;
kTriSeg.mDir = TriEdge0;
fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg);
if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
}
else // region 3
{
// min on face s=0
kTriSeg.mOrig = p0;
kTriSeg.mDir = TriEdge1;
fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
}
}
else if ( fT < 0.0f ) // region 5
{
// min on face t=0
kTriSeg.mOrig = p0;
kTriSeg.mDir = TriEdge0;
fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
}
else // region 0
{
// global minimum is interior, done
fSqrDist = fR*(fA00*fR+fA01*fS+fA02*fT+2.0f*fB0)
+fS*(fA01*fR+fA11*fS+fA12*fT+2.0f*fB1)
+fT*(fA02*fR+fA12*fS+fA22*fT+2.0f*fB2)
+kDiff.SquareMagnitude();
}
}
else
{
if ( fS < 0.0f ) // region 2
{
// min on face s=0 or s+t=1
kTriSeg.mOrig = p0;
kTriSeg.mDir = TriEdge1;
fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
kTriSeg.mOrig = p1;
kTriSeg.mDir = TriEdge1-TriEdge0;
fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg);
if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
}
else if ( fT < 0.0f ) // region 6
{
// min on face t=0 or s+t=1
kTriSeg.mOrig = p0;
kTriSeg.mDir = TriEdge0;
fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
kTriSeg.mOrig = p1;
kTriSeg.mDir = TriEdge1-TriEdge0;
fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg);
if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
}
else // region 1
{
// min on face s+t=1
kTriSeg.mOrig = p1;
kTriSeg.mDir = TriEdge1-TriEdge0;
fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
}
}
}
else // fR > 1
{
if ( fS+fT <= 1.0f )
{
if ( fS < 0.0f )
{
if ( fT < 0.0f ) // region 4p
{
// min on face s=0 or t=0 or r=1
kTriSeg.mOrig = p0;
kTriSeg.mDir = TriEdge1;
fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
kTriSeg.mOrig = p0;
kTriSeg.mDir = TriEdge0;
fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg);
if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
kPt = rkSegOrigin+rkSegDirection;
fSqrDist0 = OPC_PointTriangleSqrDist(kPt, p0, p1, p2);
if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
}
else // region 3p
{
// min on face s=0 or r=1
kTriSeg.mOrig = p0;
kTriSeg.mDir = TriEdge1;
fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
kPt = rkSegOrigin+rkSegDirection;
fSqrDist0 = OPC_PointTriangleSqrDist(kPt, p0, p1, p2);
if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
}
}
else if ( fT < 0.0f ) // region 5p
{
// min on face t=0 or r=1
kTriSeg.mOrig = p0;
kTriSeg.mDir = TriEdge0;
fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
kPt = rkSegOrigin+rkSegDirection;
fSqrDist0 = OPC_PointTriangleSqrDist(kPt, p0, p1, p2);
if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
}
else // region 0p
{
// min face on r=1
kPt = rkSegOrigin+rkSegDirection;
fSqrDist = OPC_PointTriangleSqrDist(kPt, p0, p1, p2);
}
}
else
{
if ( fS < 0.0f ) // region 2p
{
// min on face s=0 or s+t=1 or r=1
kTriSeg.mOrig = p0;
kTriSeg.mDir = TriEdge1;
fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
kTriSeg.mOrig = p1;
kTriSeg.mDir = TriEdge1-TriEdge0;
fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg);
if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
kPt = rkSegOrigin+rkSegDirection;
fSqrDist0 = OPC_PointTriangleSqrDist(kPt, p0, p1, p2);
if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
}
else if ( fT < 0.0f ) // region 6p
{
// min on face t=0 or s+t=1 or r=1
kTriSeg.mOrig = p0;
kTriSeg.mDir = TriEdge0;
fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
kTriSeg.mOrig = p1;
kTriSeg.mDir = TriEdge1-TriEdge0;
fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg);
if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
kPt = rkSegOrigin+rkSegDirection;
fSqrDist0 = OPC_PointTriangleSqrDist(kPt, p0, p1, p2);
if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
}
else // region 1p
{
// min on face s+t=1 or r=1
kTriSeg.mOrig = p1;
kTriSeg.mDir = TriEdge1-TriEdge0;
fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
kPt = rkSegOrigin+rkSegDirection;
fSqrDist0 = OPC_PointTriangleSqrDist(kPt, p0, p1, p2);
if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
}
}
}
}
else
{
// segment and triangle are parallel
kTriSeg.mOrig = p0;
kTriSeg.mDir = TriEdge0;
fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
kTriSeg.mDir = TriEdge1;
fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg);
if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
kTriSeg.mOrig = p1;
kTriSeg.mDir = TriEdge1 - TriEdge0;
fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg);
if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
fSqrDist0 = OPC_PointTriangleSqrDist(rkSegOrigin, p0, p1, p2);
if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
kPt = rkSegOrigin+rkSegDirection;
fSqrDist0 = OPC_PointTriangleSqrDist(kPt, p0, p1, p2);
if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
}
return fabsf(fSqrDist);
}
inline_ BOOL LSSCollider::LSSTriOverlap(const Point& vert0, const Point& vert1, const Point& vert2)
{
// Stats
mNbVolumePrimTests++;
float s2 = OPC_SegmentTriangleSqrDist(mSeg, vert0, vert1, vert2);
if(s2<mRadius2) return TRUE;
return FALSE;
}

303
ode/OPCODE/OPC_MeshInterface.cpp Normal file
View File

@ -0,0 +1,303 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains a mesh interface.
* \file OPC_MeshInterface.cpp
* \author Pierre Terdiman
* \date November, 27, 2002
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* This structure holds 3 vertex-pointers. It's mainly used by collision callbacks so that the app doesn't have
* to return 3 vertices to OPCODE (36 bytes) but only 3 pointers (12 bytes). It seems better but I never profiled
* the alternative.
*
* \class VertexPointers
* \author Pierre Terdiman
* \version 1.3
* \date March, 20, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* This class is an interface between us and user-defined meshes. Meshes can be defined in a lot of ways, and here we
* try to support most of them.
*
* Basically you have two options:
* - callbacks, if OPC_USE_CALLBACKS is defined in OPC_Settings.h.
* - else pointers.
*
* If using pointers, you can also use strides or not. Strides are used when OPC_USE_STRIDE is defined.
*
*
* CALLBACKS:
*
* Using callbacks is the most generic way to feed OPCODE with your meshes. Indeed, you just have to give
* access to three vertices at the end of the day. It's up to you to fetch them from your database, using
* whatever method you want. Hence your meshes can lie in system memory or AGP, be indexed or not, use 16
* or 32-bits indices, you can decompress them on-the-fly if needed, etc. On the other hand, a callback is
* called each time OPCODE needs access to a particular triangle, so there might be a slight overhead.
*
* To make things clear: geometry & topology are NOT stored in the collision system,
* in order to save some ram. So, when the system needs them to perform accurate intersection
* tests, you're requested to provide the triangle-vertices corresponding to a given face index.
*
* Ex:
*
* \code
* static void ColCallback(udword triangle_index, VertexPointers& triangle, udword user_data)
* {
* // Get back Mesh0 or Mesh1 (you also can use 2 different callbacks)
* Mesh* MyMesh = (Mesh*)user_data;
* // Get correct triangle in the app-controlled database
* const Triangle* Tri = MyMesh->GetTriangle(triangle_index);
* // Setup pointers to vertices for the collision system
* triangle.Vertex[0] = MyMesh->GetVertex(Tri->mVRef[0]);
* triangle.Vertex[1] = MyMesh->GetVertex(Tri->mVRef[1]);
* triangle.Vertex[2] = MyMesh->GetVertex(Tri->mVRef[2]);
* }
*
* // Setup callbacks
* MeshInterface0->SetCallback(ColCallback, udword(Mesh0));
* MeshInterface1->SetCallback(ColCallback, udword(Mesh1));
* \endcode
*
* Of course, you should make this callback as fast as possible. And you're also not supposed
* to modify the geometry *after* the collision trees have been built. The alternative was to
* store the geometry & topology in the collision system as well (as in RAPID) but we have found
* this approach to waste a lot of ram in many cases.
*
*
* POINTERS:
*
* If you're internally using the following canonical structures:
* - a vertex made of three 32-bits floating point values
* - a triangle made of three 32-bits integer vertex references
* ...then you may want to use pointers instead of callbacks. This is the same, except OPCODE will directly
* use provided pointers to access the topology and geometry, without using a callback. It might be faster,
* but probably not as safe. Pointers have been introduced in OPCODE 1.2.
*
* Ex:
*
* \code
* // Setup pointers
* MeshInterface0->SetPointers(Mesh0->GetFaces(), Mesh0->GetVerts());
* MeshInterface1->SetPointers(Mesh1->GetFaces(), Mesh1->GetVerts());
* \endcode
*
*
* STRIDES:
*
* If your vertices are D3D-like entities interleaving a position, a normal and/or texture coordinates
* (i.e. if your vertices are FVFs), you might want to use a vertex stride to skip extra data OPCODE
* doesn't need. Using a stride shouldn't be notably slower than not using it, but it might increase
* cache misses. Please also note that you *shouldn't* read from AGP or video-memory buffers !
*
*
* In any case, compilation flags are here to select callbacks/pointers/strides at compile time, so
* choose what's best for your application. All of this has been wrapped into this MeshInterface.
*
* \class MeshInterface
* \author Pierre Terdiman
* \version 1.3
* \date November, 27, 2002
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Precompiled Header
#include "Stdafx.h"
using namespace Opcode;
Point MeshInterface::VertexCache[3];
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Constructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
MeshInterface::MeshInterface() :
#ifdef OPC_USE_CALLBACKS
mUserData (null),
mObjCallback (null),
#else
mTris (null),
mVerts (null),
#ifdef OPC_USE_STRIDE
mTriStride (sizeof(IndexedTriangle)),
mVertexStride (sizeof(Point)),
#endif
#endif
mNbTris (0),
mNbVerts (0),
Single(true)
{
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Destructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
MeshInterface::~MeshInterface()
{
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Checks the mesh interface is valid, i.e. things have been setup correctly.
* \return true if valid
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool MeshInterface::IsValid() const
{
if(!mNbTris || !mNbVerts) return false;
#ifdef OPC_USE_CALLBACKS
if(!mObjCallback) return false;
#else
if(!mTris || !mVerts) return false;
#endif
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Checks the mesh itself is valid.
* Currently we only look for degenerate faces.
* \return number of degenerate faces
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
udword MeshInterface::CheckTopology() const
{
// Check topology. If the model contains degenerate faces, collision report can be wrong in some cases.
// e.g. it happens with the standard MAX teapot. So clean your meshes first... If you don't have a mesh cleaner
// you can try this: www.codercorner.com/Consolidation.zip
udword NbDegenerate = 0;
VertexPointers VP;
// Using callbacks, we don't have access to vertex indices. Nevertheless we still can check for
// redundant vertex pointers, which cover all possibilities (callbacks/pointers/strides).
for(udword i=0;i<mNbTris;i++)
{
GetTriangle(VP, i);
if( (VP.Vertex[0]==VP.Vertex[1])
|| (VP.Vertex[1]==VP.Vertex[2])
|| (VP.Vertex[2]==VP.Vertex[0])) NbDegenerate++;
}
return NbDegenerate;
}
#ifdef OPC_USE_CALLBACKS
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Callback control: setups object callback. Must provide triangle-vertices for a given triangle index.
* \param callback [in] user-defined callback
* \param user_data [in] user-defined data
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool MeshInterface::SetCallback(RequestCallback callback, void* user_data)
{
if(!callback) return SetIceError("MeshInterface::SetCallback: callback pointer is null");
mObjCallback = callback;
mUserData = user_data;
return true;
}
#else
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Pointers control: setups object pointers. Must provide access to faces and vertices for a given object.
* \param tris [in] pointer to triangles
* \param verts [in] pointer to vertices
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool MeshInterface::SetPointers(const IndexedTriangle* tris, const Point* verts)
{
if(!tris || !verts) return SetIceError("MeshInterface::SetPointers: pointer is null", null);
mTris = tris;
mVerts = verts;
return true;
}
#ifdef OPC_USE_STRIDE
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Strides control
* \param tri_stride [in] size of a triangle in bytes. The first sizeof(IndexedTriangle) bytes are used to get vertex indices.
* \param vertex_stride [in] size of a vertex in bytes. The first sizeof(Point) bytes are used to get vertex position.
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool MeshInterface::SetStrides(udword tri_stride, udword vertex_stride)
{
if(tri_stride<sizeof(IndexedTriangle)) return SetIceError("MeshInterface::SetStrides: invalid triangle stride", null);
if(vertex_stride<sizeof(Point)) return SetIceError("MeshInterface::SetStrides: invalid vertex stride", null);
mTriStride = tri_stride;
mVertexStride = vertex_stride;
return true;
}
#endif
#endif
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Remaps client's mesh according to a permutation.
* \param nb_indices [in] number of indices in the permutation (will be checked against number of triangles)
* \param permutation [in] list of triangle indices
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool MeshInterface::RemapClient(udword nb_indices, const udword* permutation) const
{
// Checkings
if(!nb_indices || !permutation) return false;
if(nb_indices!=mNbTris) return false;
#ifdef OPC_USE_CALLBACKS
// We can't really do that using callbacks
return false;
#else
IndexedTriangle* Tmp = new IndexedTriangle[mNbTris];
CHECKALLOC(Tmp);
#ifdef OPC_USE_STRIDE
udword Stride = mTriStride;
#else
udword Stride = sizeof(IndexedTriangle);
#endif
for(udword i=0;i<mNbTris;i++)
{
const IndexedTriangle* T = (const IndexedTriangle*)(((ubyte*)mTris) + i * Stride);
Tmp[i] = *T;
}
for(udword i=0;i<mNbTris;i++)
{
IndexedTriangle* T = (IndexedTriangle*)(((ubyte*)mTris) + i * Stride);
*T = Tmp[permutation[i]];
}
DELETEARRAY(Tmp);
#endif
return true;
}

199
ode/OPCODE/OPC_MeshInterface.h Normal file
View File

@ -0,0 +1,199 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains a mesh interface.
* \file OPC_MeshInterface.h
* \author Pierre Terdiman
* \date November, 27, 2002
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __OPC_MESHINTERFACE_H__
#define __OPC_MESHINTERFACE_H__
struct VertexPointers
{
const Point* Vertex[3];
bool BackfaceCulling(const Point& source)
{
const Point& p0 = *Vertex[0];
const Point& p1 = *Vertex[1];
const Point& p2 = *Vertex[2];
// Compute normal direction
Point Normal = (p2 - p1)^(p0 - p1);
// Backface culling
return (Normal | (source - p0)) >= 0.0f;
}
};
#ifdef OPC_USE_CALLBACKS
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* User-callback, called by OPCODE to request vertices from the app.
* \param triangle_index [in] face index for which the system is requesting the vertices
* \param triangle [out] triangle's vertices (must be provided by the user)
* \param user_data [in] user-defined data from SetCallback()
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
typedef void (*RequestCallback) (udword triangle_index, VertexPointers& triangle, void* user_data);
#endif
class OPCODE_API MeshInterface
{
public:
// Constructor / Destructor
MeshInterface();
~MeshInterface();
// Common settings
inline_ udword GetNbTriangles() const { return mNbTris; }
inline_ udword GetNbVertices() const { return mNbVerts; }
inline_ void SetNbTriangles(udword nb) { mNbTris = nb; }
inline_ void SetNbVertices(udword nb) { mNbVerts = nb; }
#ifdef OPC_USE_CALLBACKS
// Callback settings
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Callback control: setups object callback. Must provide triangle-vertices for a given triangle index.
* \param callback [in] user-defined callback
* \param user_data [in] user-defined data
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool SetCallback(RequestCallback callback, void* user_data);
inline_ void* GetUserData() const { return mUserData; }
inline_ RequestCallback GetCallback() const { return mObjCallback; }
#else
// Pointers settings
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Pointers control: setups object pointers. Must provide access to faces and vertices for a given object.
* \param tris [in] pointer to triangles
* \param verts [in] pointer to vertices
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool SetPointers(const IndexedTriangle* tris, const Point* verts);
inline_ const IndexedTriangle* GetTris() const { return mTris; }
inline_ const Point* GetVerts() const { return mVerts; }
#ifdef OPC_USE_STRIDE
// Strides settings
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Strides control
* \param tri_stride [in] size of a triangle in bytes. The first sizeof(IndexedTriangle) bytes are used to get vertex indices.
* \param vertex_stride [in] size of a vertex in bytes. The first sizeof(Point) bytes are used to get vertex position.
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool SetStrides(udword tri_stride=sizeof(IndexedTriangle), udword vertex_stride=sizeof(Point));
inline_ udword GetTriStride() const { return mTriStride; }
inline_ udword GetVertexStride() const { return mVertexStride; }
#endif
#endif
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Fetches a triangle given a triangle index.
* \param vp [out] required triangle's vertex pointers
* \param index [in] triangle index
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ void GetTriangle(VertexPointers& vp, udword index) const
{
#ifdef OPC_USE_CALLBACKS
(mObjCallback)(index, vp, mUserData);
#else
#ifdef OPC_USE_STRIDE
const IndexedTriangle* T = (const IndexedTriangle*)(((ubyte*)mTris) + index * mTriStride);
if (Single){
vp.Vertex[0] = (const Point*)(((ubyte*)mVerts) + T->mVRef[0] * mVertexStride);
vp.Vertex[1] = (const Point*)(((ubyte*)mVerts) + T->mVRef[1] * mVertexStride);
vp.Vertex[2] = (const Point*)(((ubyte*)mVerts) + T->mVRef[2] * mVertexStride);
}
else{
for (int i = 0; i < 3; i++){
const double* v = (const double*)(((ubyte*)mVerts) + T->mVRef[i] * mVertexStride);
VertexCache[i].x = (float)v[0];
VertexCache[i].y = (float)v[1];
VertexCache[i].z = (float)v[2];
vp.Vertex[i] = &VertexCache[i];
}
}
#else
const IndexedTriangle* T = &mTris[index];
vp.Vertex[0] = &mVerts[T->mVRef[0]];
vp.Vertex[1] = &mVerts[T->mVRef[1]];
vp.Vertex[2] = &mVerts[T->mVRef[2]];
#endif
#endif
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Remaps client's mesh according to a permutation.
* \param nb_indices [in] number of indices in the permutation (will be checked against number of triangles)
* \param permutation [in] list of triangle indices
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool RemapClient(udword nb_indices, const udword* permutation) const;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Checks the mesh interface is valid, i.e. things have been setup correctly.
* \return true if valid
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool IsValid() const;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Checks the mesh itself is valid.
* Currently we only look for degenerate faces.
* \return number of degenerate faces
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
udword CheckTopology() const;
private:
udword mNbTris; //!< Number of triangles in the input model
udword mNbVerts; //!< Number of vertices in the input model
#ifdef OPC_USE_CALLBACKS
// User callback
void* mUserData; //!< User-defined data sent to callback
RequestCallback mObjCallback; //!< Object callback
#else
// User pointers
const IndexedTriangle* mTris; //!< Array of indexed triangles
const Point* mVerts; //!< Array of vertices
#ifdef OPC_USE_STRIDE
udword mTriStride; //!< Possible triangle stride in bytes [Opcode 1.3]
udword mVertexStride; //!< Possible vertex stride in bytes [Opcode 1.3]
#endif
public:
bool Single; //!< Use single or double precision vertices
private:
static Point VertexCache[3];
#endif
};
#endif //__OPC_MESHINTERFACE_H__

222
ode/OPCODE/OPC_Model.cpp Normal file
View File

@ -0,0 +1,222 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for OPCODE models.
* \file OPC_Model.cpp
* \author Pierre Terdiman
* \date March, 20, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* The main collision wrapper, for all trees. Supported trees are:
* - Normal trees (2*N-1 nodes, full size)
* - No-leaf trees (N-1 nodes, full size)
* - Quantized trees (2*N-1 nodes, half size)
* - Quantized no-leaf trees (N-1 nodes, half size)
*
* Usage:
*
* 1) Create a static mesh interface using callbacks or pointers. (see OPC_MeshInterface.cpp).
* Keep it around in your app, since a pointer to this interface is saved internally and
* used until you release the collision structures.
*
* 2) Build a Model using a creation structure:
*
* \code
* Model Sample;
*
* OPCODECREATE OPCC;
* OPCC.IMesh = ...;
* OPCC.Rules = ...;
* OPCC.NoLeaf = ...;
* OPCC.Quantized = ...;
* OPCC.KeepOriginal = ...;
* bool Status = Sample.Build(OPCC);
* \endcode
*
* 3) Create a tree collider and set it up:
*
* \code
* AABBTreeCollider TC;
* TC.SetFirstContact(...);
* TC.SetFullBoxBoxTest(...);
* TC.SetFullPrimBoxTest(...);
* TC.SetTemporalCoherence(...);
* \endcode
*
* 4) Perform a collision query
*
* \code
* // Setup cache
* static BVTCache ColCache;
* ColCache.Model0 = &Model0;
* ColCache.Model1 = &Model1;
*
* // Collision query
* bool IsOk = TC.Collide(ColCache, World0, World1);
*
* // Get collision status => if true, objects overlap
* BOOL Status = TC.GetContactStatus();
*
* // Number of colliding pairs and list of pairs
* udword NbPairs = TC.GetNbPairs();
* const Pair* p = TC.GetPairs()
* \endcode
*
* 5) Stats
*
* \code
* Model0.GetUsedBytes() = number of bytes used for this collision tree
* TC.GetNbBVBVTests() = number of BV-BV overlap tests performed during last query
* TC.GetNbPrimPrimTests() = number of Triangle-Triangle overlap tests performed during last query
* TC.GetNbBVPrimTests() = number of Triangle-BV overlap tests performed during last query
* \endcode
*
* \class Model
* \author Pierre Terdiman
* \version 1.3
* \date March, 20, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Precompiled Header
#include "Stdafx.h"
using namespace Opcode;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Constructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Model::Model()
{
#ifdef __MESHMERIZER_H__ // Collision hulls only supported within ICE !
mHull = null;
#endif // __MESHMERIZER_H__
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Destructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Model::~Model()
{
Release();
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Releases the model.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void Model::Release()
{
ReleaseBase();
#ifdef __MESHMERIZER_H__ // Collision hulls only supported within ICE !
DELETESINGLE(mHull);
#endif // __MESHMERIZER_H__
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Builds a collision model.
* \param create [in] model creation structure
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool Model::Build(const OPCODECREATE& create)
{
// 1) Checkings
if(!create.mIMesh || !create.mIMesh->IsValid()) return false;
// For this model, we only support complete trees
if(create.mSettings.mLimit!=1) return SetIceError("OPCODE WARNING: supports complete trees only! Use mLimit = 1.\n", null);
// Look for degenerate faces.
udword NbDegenerate = create.mIMesh->CheckTopology();
if(NbDegenerate) Log("OPCODE WARNING: found %d degenerate faces in model! Collision might report wrong results!\n", NbDegenerate);
// We continue nonetheless....
Release(); // Make sure previous tree has been discarded [Opcode 1.3, thanks Adam]
// 1-1) Setup mesh interface automatically [Opcode 1.3]
SetMeshInterface(create.mIMesh);
// Special case for 1-triangle meshes [Opcode 1.3]
udword NbTris = create.mIMesh->GetNbTriangles();
if(NbTris==1)
{
// We don't need to actually create a tree here, since we'll only have a single triangle to deal with anyway.
// It's a waste to use a "model" for this but at least it will work.
mModelCode |= OPC_SINGLE_NODE;
return true;
}
// 2) Build a generic AABB Tree.
mSource = new AABBTree;
CHECKALLOC(mSource);
// 2-1) Setup a builder. Our primitives here are triangles from input mesh,
// so we use an AABBTreeOfTrianglesBuilder.....
{
AABBTreeOfTrianglesBuilder TB;
TB.mIMesh = create.mIMesh;
TB.mSettings = create.mSettings;
TB.mNbPrimitives = NbTris;
if(!mSource->Build(&TB)) return false;
}
// 3) Create an optimized tree according to user-settings
if(!CreateTree(create.mNoLeaf, create.mQuantized)) return false;
// 3-2) Create optimized tree
if(!mTree->Build(mSource)) return false;
// 3-3) Delete generic tree if needed
if(!create.mKeepOriginal) DELETESINGLE(mSource);
#ifdef __MESHMERIZER_H__
// 4) Convex hull
if(create.mCollisionHull)
{
// Create hull
mHull = new CollisionHull;
CHECKALLOC(mHull);
CONVEXHULLCREATE CHC;
// ### doesn't work with strides
CHC.NbVerts = create.mIMesh->GetNbVertices();
CHC.Vertices = create.mIMesh->GetVerts();
CHC.UnifyNormals = true;
CHC.ReduceVertices = true;
CHC.WordFaces = false;
mHull->Compute(CHC);
}
#endif // __MESHMERIZER_H__
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Gets the number of bytes used by the tree.
* \return amount of bytes used
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
udword Model::GetUsedBytes() const
{
if(!mTree) return 0;
return mTree->GetUsedBytes();
}

65
ode/OPCODE/OPC_Model.h Normal file
View File

@ -0,0 +1,65 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for OPCODE models.
* \file OPC_Model.h
* \author Pierre Terdiman
* \date March, 20, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __OPC_MODEL_H__
#define __OPC_MODEL_H__
class OPCODE_API Model : public BaseModel
{
public:
// Constructor/Destructor
Model();
virtual ~Model();
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Builds a collision model.
* \param create [in] model creation structure
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
override(BaseModel) bool Build(const OPCODECREATE& create);
#ifdef __MESHMERIZER_H__
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Gets the collision hull.
* \return the collision hull if it exists
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ const CollisionHull* GetHull() const { return mHull; }
#endif // __MESHMERIZER_H__
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Gets the number of bytes used by the tree.
* \return amount of bytes used
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
override(BaseModel) udword GetUsedBytes() const;
private:
#ifdef __MESHMERIZER_H__
CollisionHull* mHull; //!< Possible convex hull
#endif // __MESHMERIZER_H__
// Internal methods
void Release();
};
#endif //__OPC_MODEL_H__

767
ode/OPCODE/OPC_OBBCollider.cpp Normal file
View File

@ -0,0 +1,767 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for an OBB collider.
* \file OPC_OBBCollider.cpp
* \author Pierre Terdiman
* \date January, 1st, 2002
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains an OBB-vs-tree collider.
*
* \class OBBCollider
* \author Pierre Terdiman
* \version 1.3
* \date January, 1st, 2002
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Precompiled Header
#include "Stdafx.h"
using namespace Opcode;
#include "OPC_BoxBoxOverlap.h"
#include "OPC_TriBoxOverlap.h"
#define SET_CONTACT(prim_index, flag) \
/* Set contact status */ \
mFlags |= flag; \
mTouchedPrimitives->Add(udword(prim_index));
//! OBB-triangle test
#define OBB_PRIM(prim_index, flag) \
/* Request vertices from the app */ \
VertexPointers VP; mIMesh->GetTriangle(VP, prim_index); \
/* Transform them in a common space */ \
TransformPoint(mLeafVerts[0], *VP.Vertex[0], mRModelToBox, mTModelToBox); \
TransformPoint(mLeafVerts[1], *VP.Vertex[1], mRModelToBox, mTModelToBox); \
TransformPoint(mLeafVerts[2], *VP.Vertex[2], mRModelToBox, mTModelToBox); \
/* Perform triangle-box overlap test */ \
if(TriBoxOverlap()) \
{ \
SET_CONTACT(prim_index, flag) \
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Constructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
OBBCollider::OBBCollider() : mFullBoxBoxTest(true)
{
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Destructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
OBBCollider::~OBBCollider()
{
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Validates current settings. You should call this method after all the settings and callbacks have been defined.
* \return null if everything is ok, else a string describing the problem
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
const char* OBBCollider::ValidateSettings()
{
if(TemporalCoherenceEnabled() && !FirstContactEnabled()) return "Temporal coherence only works with ""First contact"" mode!";
return VolumeCollider::ValidateSettings();
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Generic collision query for generic OPCODE models. After the call, access the results:
* - with GetContactStatus()
* - with GetNbTouchedPrimitives()
* - with GetTouchedPrimitives()
*
* \param cache [in/out] a box cache
* \param box [in] collision OBB in local space
* \param model [in] Opcode model to collide with
* \param worldb [in] OBB's world matrix, or null
* \param worldm [in] model's world matrix, or null
* \return true if success
* \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool OBBCollider::Collide(OBBCache& cache, const OBB& box, const Model& model, const Matrix4x4* worldb, const Matrix4x4* worldm)
{
// Checkings
if(!Setup(&model)) return false;
// Init collision query
if(InitQuery(cache, box, worldb, worldm)) return true;
if(!model.HasLeafNodes())
{
if(model.IsQuantized())
{
const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree();
// Setup dequantization coeffs
mCenterCoeff = Tree->mCenterCoeff;
mExtentsCoeff = Tree->mExtentsCoeff;
// Perform collision query
if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes());
else _Collide(Tree->GetNodes());
}
else
{
const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree();
// Perform collision query
if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes());
else _Collide(Tree->GetNodes());
}
}
else
{
if(model.IsQuantized())
{
const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree();
// Setup dequantization coeffs
mCenterCoeff = Tree->mCenterCoeff;
mExtentsCoeff = Tree->mExtentsCoeff;
// Perform collision query
if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes());
else _Collide(Tree->GetNodes());
}
else
{
const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree();
// Perform collision query
if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes());
else _Collide(Tree->GetNodes());
}
}
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Initializes a collision query :
* - reset stats & contact status
* - setup matrices
* - check temporal coherence
*
* \param cache [in/out] a box cache
* \param box [in] obb in local space
* \param worldb [in] obb's world matrix, or null
* \param worldm [in] model's world matrix, or null
* \return TRUE if we can return immediately
* \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
BOOL OBBCollider::InitQuery(OBBCache& cache, const OBB& box, const Matrix4x4* worldb, const Matrix4x4* worldm)
{
// 1) Call the base method
VolumeCollider::InitQuery();
// 2) Compute obb in world space
mBoxExtents = box.mExtents;
Matrix4x4 WorldB;
if(worldb)
{
WorldB = Matrix4x4( box.mRot * Matrix3x3(*worldb) );
WorldB.SetTrans(box.mCenter * *worldb);
}
else
{
WorldB = box.mRot;
WorldB.SetTrans(box.mCenter);
}
// Setup matrices
Matrix4x4 InvWorldB;
InvertPRMatrix(InvWorldB, WorldB);
if(worldm)
{
Matrix4x4 InvWorldM;
InvertPRMatrix(InvWorldM, *worldm);
Matrix4x4 WorldBtoM = WorldB * InvWorldM;
Matrix4x4 WorldMtoB = *worldm * InvWorldB;
mRModelToBox = WorldMtoB; WorldMtoB.GetTrans(mTModelToBox);
mRBoxToModel = WorldBtoM; WorldBtoM.GetTrans(mTBoxToModel);
}
else
{
mRModelToBox = InvWorldB; InvWorldB.GetTrans(mTModelToBox);
mRBoxToModel = WorldB; WorldB.GetTrans(mTBoxToModel);
}
// 3) Setup destination pointer
mTouchedPrimitives = &cache.TouchedPrimitives;
// 4) Special case: 1-triangle meshes [Opcode 1.3]
if(mCurrentModel && mCurrentModel->HasSingleNode())
{
if(!SkipPrimitiveTests())
{
// We simply perform the BV-Prim overlap test each time. We assume single triangle has index 0.
mTouchedPrimitives->Reset();
// Perform overlap test between the unique triangle and the box (and set contact status if needed)
OBB_PRIM(udword(0), OPC_CONTACT)
// Return immediately regardless of status
return TRUE;
}
}
// 5) Check temporal coherence:
if(TemporalCoherenceEnabled())
{
// Here we use temporal coherence
// => check results from previous frame before performing the collision query
if(FirstContactEnabled())
{
// We're only interested in the first contact found => test the unique previously touched face
if(mTouchedPrimitives->GetNbEntries())
{
// Get index of previously touched face = the first entry in the array
udword PreviouslyTouchedFace = mTouchedPrimitives->GetEntry(0);
// Then reset the array:
// - if the overlap test below is successful, the index we'll get added back anyway
// - if it isn't, then the array should be reset anyway for the normal query
mTouchedPrimitives->Reset();
// Perform overlap test between the cached triangle and the box (and set contact status if needed)
OBB_PRIM(PreviouslyTouchedFace, OPC_TEMPORAL_CONTACT)
// Return immediately if possible
if(GetContactStatus()) return TRUE;
}
// else no face has been touched during previous query
// => we'll have to perform a normal query
}
else
{
// ### rewrite this
OBB TestBox(mTBoxToModel, mBoxExtents, mRBoxToModel);
// We're interested in all contacts =>test the new real box N(ew) against the previous fat box P(revious):
if(IsCacheValid(cache) && TestBox.IsInside(cache.FatBox))
{
// - if N is included in P, return previous list
// => we simply leave the list (mTouchedFaces) unchanged
// Set contact status if needed
if(mTouchedPrimitives->GetNbEntries()) mFlags |= OPC_TEMPORAL_CONTACT;
// In any case we don't need to do a query
return TRUE;
}
else
{
// - else do the query using a fat N
// Reset cache since we'll about to perform a real query
mTouchedPrimitives->Reset();
// Make a fat box so that coherence will work for subsequent frames
TestBox.mExtents *= cache.FatCoeff;
mBoxExtents *= cache.FatCoeff;
// Update cache with query data (signature for cached faces)
cache.FatBox = TestBox;
}
}
}
else
{
// Here we don't use temporal coherence => do a normal query
mTouchedPrimitives->Reset();
}
// Now we can precompute box-box data
// Precompute absolute box-to-model rotation matrix
for(udword i=0;i<3;i++)
{
for(udword j=0;j<3;j++)
{
// Epsilon value prevents floating-point inaccuracies (strategy borrowed from RAPID)
mAR.m[i][j] = 1e-6f + fabsf(mRBoxToModel.m[i][j]);
}
}
// Precompute bounds for box-in-box test
mB0 = mBoxExtents - mTModelToBox;
mB1 = - mBoxExtents - mTModelToBox;
// Precompute box-box data - Courtesy of Erwin de Vries
mBBx1 = mBoxExtents.x*mAR.m[0][0] + mBoxExtents.y*mAR.m[1][0] + mBoxExtents.z*mAR.m[2][0];
mBBy1 = mBoxExtents.x*mAR.m[0][1] + mBoxExtents.y*mAR.m[1][1] + mBoxExtents.z*mAR.m[2][1];
mBBz1 = mBoxExtents.x*mAR.m[0][2] + mBoxExtents.y*mAR.m[1][2] + mBoxExtents.z*mAR.m[2][2];
mBB_1 = mBoxExtents.y*mAR.m[2][0] + mBoxExtents.z*mAR.m[1][0];
mBB_2 = mBoxExtents.x*mAR.m[2][0] + mBoxExtents.z*mAR.m[0][0];
mBB_3 = mBoxExtents.x*mAR.m[1][0] + mBoxExtents.y*mAR.m[0][0];
mBB_4 = mBoxExtents.y*mAR.m[2][1] + mBoxExtents.z*mAR.m[1][1];
mBB_5 = mBoxExtents.x*mAR.m[2][1] + mBoxExtents.z*mAR.m[0][1];
mBB_6 = mBoxExtents.x*mAR.m[1][1] + mBoxExtents.y*mAR.m[0][1];
mBB_7 = mBoxExtents.y*mAR.m[2][2] + mBoxExtents.z*mAR.m[1][2];
mBB_8 = mBoxExtents.x*mAR.m[2][2] + mBoxExtents.z*mAR.m[0][2];
mBB_9 = mBoxExtents.x*mAR.m[1][2] + mBoxExtents.y*mAR.m[0][2];
return FALSE;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Checks the OBB completely contains the box. In which case we can end the query sooner.
* \param bc [in] box center
* \param be [in] box extents
* \return true if the OBB contains the whole box
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ BOOL OBBCollider::OBBContainsBox(const Point& bc, const Point& be)
{
// I assume if all 8 box vertices are inside the OBB, so does the whole box.
// Sounds ok but maybe there's a better way?
/*
#define TEST_PT(a,b,c) \
p.x=a; p.y=b; p.z=c; p+=bc; \
f = p.x * mRModelToBox.m[0][0] + p.y * mRModelToBox.m[1][0] + p.z * mRModelToBox.m[2][0]; if(f>mB0.x || f<mB1.x) return FALSE;\
f = p.x * mRModelToBox.m[0][1] + p.y * mRModelToBox.m[1][1] + p.z * mRModelToBox.m[2][1]; if(f>mB0.y || f<mB1.y) return FALSE;\
f = p.x * mRModelToBox.m[0][2] + p.y * mRModelToBox.m[1][2] + p.z * mRModelToBox.m[2][2]; if(f>mB0.z || f<mB1.z) return FALSE;
Point p;
float f;
TEST_PT(be.x, be.y, be.z)
TEST_PT(-be.x, be.y, be.z)
TEST_PT(be.x, -be.y, be.z)
TEST_PT(-be.x, -be.y, be.z)
TEST_PT(be.x, be.y, -be.z)
TEST_PT(-be.x, be.y, -be.z)
TEST_PT(be.x, -be.y, -be.z)
TEST_PT(-be.x, -be.y, -be.z)
return TRUE;
*/
// Yes there is:
// - compute model-box's AABB in OBB space
// - test AABB-in-AABB
float NCx = bc.x * mRModelToBox.m[0][0] + bc.y * mRModelToBox.m[1][0] + bc.z * mRModelToBox.m[2][0];
float NEx = fabsf(mRModelToBox.m[0][0] * be.x) + fabsf(mRModelToBox.m[1][0] * be.y) + fabsf(mRModelToBox.m[2][0] * be.z);
if(mB0.x < NCx+NEx) return FALSE;
if(mB1.x > NCx-NEx) return FALSE;
float NCy = bc.x * mRModelToBox.m[0][1] + bc.y * mRModelToBox.m[1][1] + bc.z * mRModelToBox.m[2][1];
float NEy = fabsf(mRModelToBox.m[0][1] * be.x) + fabsf(mRModelToBox.m[1][1] * be.y) + fabsf(mRModelToBox.m[2][1] * be.z);
if(mB0.y < NCy+NEy) return FALSE;
if(mB1.y > NCy-NEy) return FALSE;
float NCz = bc.x * mRModelToBox.m[0][2] + bc.y * mRModelToBox.m[1][2] + bc.z * mRModelToBox.m[2][2];
float NEz = fabsf(mRModelToBox.m[0][2] * be.x) + fabsf(mRModelToBox.m[1][2] * be.y) + fabsf(mRModelToBox.m[2][2] * be.z);
if(mB0.z < NCz+NEz) return FALSE;
if(mB1.z > NCz-NEz) return FALSE;
return TRUE;
}
#define TEST_BOX_IN_OBB(center, extents) \
if(OBBContainsBox(center, extents)) \
{ \
/* Set contact status */ \
mFlags |= OPC_CONTACT; \
_Dump(node); \
return; \
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for normal AABB trees.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void OBBCollider::_Collide(const AABBCollisionNode* node)
{
// Perform OBB-AABB overlap test
if(!BoxBoxOverlap(node->mAABB.mExtents, node->mAABB.mCenter)) return;
TEST_BOX_IN_OBB(node->mAABB.mCenter, node->mAABB.mExtents)
if(node->IsLeaf())
{
OBB_PRIM(node->GetPrimitive(), OPC_CONTACT)
}
else
{
_Collide(node->GetPos());
if(ContactFound()) return;
_Collide(node->GetNeg());
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for normal AABB trees, without primitive tests.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void OBBCollider::_CollideNoPrimitiveTest(const AABBCollisionNode* node)
{
// Perform OBB-AABB overlap test
if(!BoxBoxOverlap(node->mAABB.mExtents, node->mAABB.mCenter)) return;
TEST_BOX_IN_OBB(node->mAABB.mCenter, node->mAABB.mExtents)
if(node->IsLeaf())
{
SET_CONTACT(node->GetPrimitive(), OPC_CONTACT)
}
else
{
_CollideNoPrimitiveTest(node->GetPos());
if(ContactFound()) return;
_CollideNoPrimitiveTest(node->GetNeg());
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for quantized AABB trees.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void OBBCollider::_Collide(const AABBQuantizedNode* node)
{
// Dequantize box
const QuantizedAABB& Box = node->mAABB;
const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
// Perform OBB-AABB overlap test
if(!BoxBoxOverlap(Extents, Center)) return;
TEST_BOX_IN_OBB(Center, Extents)
if(node->IsLeaf())
{
OBB_PRIM(node->GetPrimitive(), OPC_CONTACT)
}
else
{
_Collide(node->GetPos());
if(ContactFound()) return;
_Collide(node->GetNeg());
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for quantized AABB trees, without primitive tests.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void OBBCollider::_CollideNoPrimitiveTest(const AABBQuantizedNode* node)
{
// Dequantize box
const QuantizedAABB& Box = node->mAABB;
const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
// Perform OBB-AABB overlap test
if(!BoxBoxOverlap(Extents, Center)) return;
TEST_BOX_IN_OBB(Center, Extents)
if(node->IsLeaf())
{
SET_CONTACT(node->GetPrimitive(), OPC_CONTACT)
}
else
{
_CollideNoPrimitiveTest(node->GetPos());
if(ContactFound()) return;
_CollideNoPrimitiveTest(node->GetNeg());
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for no-leaf AABB trees.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void OBBCollider::_Collide(const AABBNoLeafNode* node)
{
// Perform OBB-AABB overlap test
if(!BoxBoxOverlap(node->mAABB.mExtents, node->mAABB.mCenter)) return;
TEST_BOX_IN_OBB(node->mAABB.mCenter, node->mAABB.mExtents)
if(node->HasPosLeaf()) { OBB_PRIM(node->GetPosPrimitive(), OPC_CONTACT) }
else _Collide(node->GetPos());
if(ContactFound()) return;
if(node->HasNegLeaf()) { OBB_PRIM(node->GetNegPrimitive(), OPC_CONTACT) }
else _Collide(node->GetNeg());
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for no-leaf AABB trees, without primitive tests.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void OBBCollider::_CollideNoPrimitiveTest(const AABBNoLeafNode* node)
{
// Perform OBB-AABB overlap test
if(!BoxBoxOverlap(node->mAABB.mExtents, node->mAABB.mCenter)) return;
TEST_BOX_IN_OBB(node->mAABB.mCenter, node->mAABB.mExtents)
if(node->HasPosLeaf()) { SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) }
else _CollideNoPrimitiveTest(node->GetPos());
if(ContactFound()) return;
if(node->HasNegLeaf()) { SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) }
else _CollideNoPrimitiveTest(node->GetNeg());
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for quantized no-leaf AABB trees.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void OBBCollider::_Collide(const AABBQuantizedNoLeafNode* node)
{
// Dequantize box
const QuantizedAABB& Box = node->mAABB;
const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
// Perform OBB-AABB overlap test
if(!BoxBoxOverlap(Extents, Center)) return;
TEST_BOX_IN_OBB(Center, Extents)
if(node->HasPosLeaf()) { OBB_PRIM(node->GetPosPrimitive(), OPC_CONTACT) }
else _Collide(node->GetPos());
if(ContactFound()) return;
if(node->HasNegLeaf()) { OBB_PRIM(node->GetNegPrimitive(), OPC_CONTACT) }
else _Collide(node->GetNeg());
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for quantized no-leaf AABB trees, without primitive tests.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void OBBCollider::_CollideNoPrimitiveTest(const AABBQuantizedNoLeafNode* node)
{
// Dequantize box
const QuantizedAABB& Box = node->mAABB;
const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
// Perform OBB-AABB overlap test
if(!BoxBoxOverlap(Extents, Center)) return;
TEST_BOX_IN_OBB(Center, Extents)
if(node->HasPosLeaf()) { SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) }
else _CollideNoPrimitiveTest(node->GetPos());
if(ContactFound()) return;
if(node->HasNegLeaf()) { SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) }
else _CollideNoPrimitiveTest(node->GetNeg());
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Constructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
HybridOBBCollider::HybridOBBCollider()
{
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Destructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
HybridOBBCollider::~HybridOBBCollider()
{
}
bool HybridOBBCollider::Collide(OBBCache& cache, const OBB& box, const HybridModel& model, const Matrix4x4* worldb, const Matrix4x4* worldm)
{
// We don't want primitive tests here!
mFlags |= OPC_NO_PRIMITIVE_TESTS;
// Checkings
if(!Setup(&model)) return false;
// Init collision query
if(InitQuery(cache, box, worldb, worldm)) return true;
// Special case for 1-leaf trees
if(mCurrentModel && mCurrentModel->HasSingleNode())
{
// Here we're supposed to perform a normal query, except our tree has a single node, i.e. just a few triangles
udword Nb = mIMesh->GetNbTriangles();
// Loop through all triangles
for(udword i=0;i<Nb;i++)
{
OBB_PRIM(i, OPC_CONTACT)
}
return true;
}
// Override destination array since we're only going to get leaf boxes here
mTouchedBoxes.Reset();
mTouchedPrimitives = &mTouchedBoxes;
// Now, do the actual query against leaf boxes
if(!model.HasLeafNodes())
{
if(model.IsQuantized())
{
const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree();
// Setup dequantization coeffs
mCenterCoeff = Tree->mCenterCoeff;
mExtentsCoeff = Tree->mExtentsCoeff;
// Perform collision query - we don't want primitive tests here!
_CollideNoPrimitiveTest(Tree->GetNodes());
}
else
{
const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree();
// Perform collision query - we don't want primitive tests here!
_CollideNoPrimitiveTest(Tree->GetNodes());
}
}
else
{
if(model.IsQuantized())
{
const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree();
// Setup dequantization coeffs
mCenterCoeff = Tree->mCenterCoeff;
mExtentsCoeff = Tree->mExtentsCoeff;
// Perform collision query - we don't want primitive tests here!
_CollideNoPrimitiveTest(Tree->GetNodes());
}
else
{
const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree();
// Perform collision query - we don't want primitive tests here!
_CollideNoPrimitiveTest(Tree->GetNodes());
}
}
// We only have a list of boxes so far
if(GetContactStatus())
{
// Reset contact status, since it currently only reflects collisions with leaf boxes
Collider::InitQuery();
// Change dest container so that we can use built-in overlap tests and get collided primitives
cache.TouchedPrimitives.Reset();
mTouchedPrimitives = &cache.TouchedPrimitives;
// Read touched leaf boxes
udword Nb = mTouchedBoxes.GetNbEntries();
const udword* Touched = mTouchedBoxes.GetEntries();
const LeafTriangles* LT = model.GetLeafTriangles();
const udword* Indices = model.GetIndices();
// Loop through touched leaves
while(Nb--)
{
const LeafTriangles& CurrentLeaf = LT[*Touched++];
// Each leaf box has a set of triangles
udword NbTris = CurrentLeaf.GetNbTriangles();
if(Indices)
{
const udword* T = &Indices[CurrentLeaf.GetTriangleIndex()];
// Loop through triangles and test each of them
while(NbTris--)
{
udword TriangleIndex = *T++;
OBB_PRIM(TriangleIndex, OPC_CONTACT)
}
}
else
{
udword BaseIndex = CurrentLeaf.GetTriangleIndex();
// Loop through triangles and test each of them
while(NbTris--)
{
udword TriangleIndex = BaseIndex++;
OBB_PRIM(TriangleIndex, OPC_CONTACT)
}
}
}
}
return true;
}

142
ode/OPCODE/OPC_OBBCollider.h Normal file
View File

@ -0,0 +1,142 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for an OBB collider.
* \file OPC_OBBCollider.h
* \author Pierre Terdiman
* \date January, 1st, 2002
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __OPC_OBBCOLLIDER_H__
#define __OPC_OBBCOLLIDER_H__
struct OPCODE_API OBBCache : VolumeCache
{
OBBCache() : FatCoeff(1.1f)
{
FatBox.mCenter.Zero();
FatBox.mExtents.Zero();
FatBox.mRot.Identity();
}
// Cached faces signature
OBB FatBox; //!< Box used when performing the query resulting in cached faces
// User settings
float FatCoeff; //!< extents multiplier used to create a fat box
};
class OPCODE_API OBBCollider : public VolumeCollider
{
public:
// Constructor / Destructor
OBBCollider();
virtual ~OBBCollider();
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Generic collision query for generic OPCODE models. After the call, access the results:
* - with GetContactStatus()
* - with GetNbTouchedPrimitives()
* - with GetTouchedPrimitives()
*
* \param cache [in/out] a box cache
* \param box [in] collision OBB in local space
* \param model [in] Opcode model to collide with
* \param worldb [in] OBB's world matrix, or null
* \param worldm [in] model's world matrix, or null
* \return true if success
* \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool Collide(OBBCache& cache, const OBB& box, const Model& model, const Matrix4x4* worldb=null, const Matrix4x4* worldm=null);
// Settings
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Settings: select between full box-box tests or "SAT-lite" tests (where Class III axes are discarded)
* \param flag [in] true for full tests, false for coarse tests
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ void SetFullBoxBoxTest(bool flag) { mFullBoxBoxTest = flag; }
// Settings
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Validates current settings. You should call this method after all the settings and callbacks have been defined for a collider.
* \return null if everything is ok, else a string describing the problem
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
override(Collider) const char* ValidateSettings();
protected:
// Precomputed data
Matrix3x3 mAR; //!< Absolute rotation matrix
Matrix3x3 mRModelToBox; //!< Rotation from model space to obb space
Matrix3x3 mRBoxToModel; //!< Rotation from obb space to model space
Point mTModelToBox; //!< Translation from model space to obb space
Point mTBoxToModel; //!< Translation from obb space to model space
Point mBoxExtents;
Point mB0; //!< - mTModelToBox + mBoxExtents
Point mB1; //!< - mTModelToBox - mBoxExtents
float mBBx1;
float mBBy1;
float mBBz1;
float mBB_1;
float mBB_2;
float mBB_3;
float mBB_4;
float mBB_5;
float mBB_6;
float mBB_7;
float mBB_8;
float mBB_9;
// Leaf description
Point mLeafVerts[3]; //!< Triangle vertices
// Settings
bool mFullBoxBoxTest; //!< Perform full BV-BV tests (true) or SAT-lite tests (false)
// Internal methods
void _Collide(const AABBCollisionNode* node);
void _Collide(const AABBNoLeafNode* node);
void _Collide(const AABBQuantizedNode* node);
void _Collide(const AABBQuantizedNoLeafNode* node);
void _CollideNoPrimitiveTest(const AABBCollisionNode* node);
void _CollideNoPrimitiveTest(const AABBNoLeafNode* node);
void _CollideNoPrimitiveTest(const AABBQuantizedNode* node);
void _CollideNoPrimitiveTest(const AABBQuantizedNoLeafNode* node);
// Overlap tests
inline_ BOOL OBBContainsBox(const Point& bc, const Point& be);
inline_ BOOL BoxBoxOverlap(const Point& extents, const Point& center);
inline_ BOOL TriBoxOverlap();
// Init methods
BOOL InitQuery(OBBCache& cache, const OBB& box, const Matrix4x4* worldb=null, const Matrix4x4* worldm=null);
};
class OPCODE_API HybridOBBCollider : public OBBCollider
{
public:
// Constructor / Destructor
HybridOBBCollider();
virtual ~HybridOBBCollider();
bool Collide(OBBCache& cache, const OBB& box, const HybridModel& model, const Matrix4x4* worldb=null, const Matrix4x4* worldm=null);
protected:
Container mTouchedBoxes;
};
#endif // __OPC_OBBCOLLIDER_H__

782
ode/OPCODE/OPC_OptimizedTree.cpp Normal file
View File

@ -0,0 +1,782 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for optimized trees. Implements 4 trees:
* - normal
* - no leaf
* - quantized
* - no leaf / quantized
*
* \file OPC_OptimizedTree.cpp
* \author Pierre Terdiman
* \date March, 20, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* A standard AABB tree.
*
* \class AABBCollisionTree
* \author Pierre Terdiman
* \version 1.3
* \date March, 20, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* A no-leaf AABB tree.
*
* \class AABBNoLeafTree
* \author Pierre Terdiman
* \version 1.3
* \date March, 20, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* A quantized AABB tree.
*
* \class AABBQuantizedTree
* \author Pierre Terdiman
* \version 1.3
* \date March, 20, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* A quantized no-leaf AABB tree.
*
* \class AABBQuantizedNoLeafTree
* \author Pierre Terdiman
* \version 1.3
* \date March, 20, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Precompiled Header
#include "Stdafx.h"
using namespace Opcode;
//! Compilation flag:
//! - true to fix quantized boxes (i.e. make sure they enclose the original ones)
//! - false to see the effects of quantization errors (faster, but wrong results in some cases)
static bool gFixQuantized = true;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Builds an implicit tree from a standard one. An implicit tree is a complete tree (2*N-1 nodes) whose negative
* box pointers and primitive pointers have been made implicit, hence packing 3 pointers in one.
*
* Layout for implicit trees:
* Node:
* - box
* - data (32-bits value)
*
* if data's LSB = 1 => remaining bits are a primitive pointer
* else remaining bits are a P-node pointer, and N = P + 1
*
* \relates AABBCollisionNode
* \fn _BuildCollisionTree(AABBCollisionNode* linear, const udword box_id, udword& current_id, const AABBTreeNode* current_node)
* \param linear [in] base address of destination nodes
* \param box_id [in] index of destination node
* \param current_id [in] current running index
* \param current_node [in] current node from input tree
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
static void _BuildCollisionTree(AABBCollisionNode* linear, const udword box_id, udword& current_id, const AABBTreeNode* current_node)
{
// Current node from input tree is "current_node". Must be flattened into "linear[boxid]".
// Store the AABB
current_node->GetAABB()->GetCenter(linear[box_id].mAABB.mCenter);
current_node->GetAABB()->GetExtents(linear[box_id].mAABB.mExtents);
// Store remaining info
if(current_node->IsLeaf())
{
// The input tree must be complete => i.e. one primitive/leaf
ASSERT(current_node->GetNbPrimitives()==1);
// Get the primitive index from the input tree
udword PrimitiveIndex = current_node->GetPrimitives()[0];
// Setup box data as the primitive index, marked as leaf
linear[box_id].mData = (PrimitiveIndex<<1)|1;
}
else
{
// To make the negative one implicit, we must store P and N in successive order
udword PosID = current_id++; // Get a new id for positive child
udword NegID = current_id++; // Get a new id for negative child
// Setup box data as the forthcoming new P pointer
linear[box_id].mData = (size_t)&linear[PosID];
// Make sure it's not marked as leaf
ASSERT(!(linear[box_id].mData&1));
// Recurse with new IDs
_BuildCollisionTree(linear, PosID, current_id, current_node->GetPos());
_BuildCollisionTree(linear, NegID, current_id, current_node->GetNeg());
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Builds a "no-leaf" tree from a standard one. This is a tree whose leaf nodes have been removed.
*
* Layout for no-leaf trees:
*
* Node:
* - box
* - P pointer => a node (LSB=0) or a primitive (LSB=1)
* - N pointer => a node (LSB=0) or a primitive (LSB=1)
*
* \relates AABBNoLeafNode
* \fn _BuildNoLeafTree(AABBNoLeafNode* linear, const udword box_id, udword& current_id, const AABBTreeNode* current_node)
* \param linear [in] base address of destination nodes
* \param box_id [in] index of destination node
* \param current_id [in] current running index
* \param current_node [in] current node from input tree
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
static void _BuildNoLeafTree(AABBNoLeafNode* linear, const udword box_id, udword& current_id, const AABBTreeNode* current_node)
{
const AABBTreeNode* P = current_node->GetPos();
const AABBTreeNode* N = current_node->GetNeg();
// Leaf nodes here?!
ASSERT(P);
ASSERT(N);
// Internal node => keep the box
current_node->GetAABB()->GetCenter(linear[box_id].mAABB.mCenter);
current_node->GetAABB()->GetExtents(linear[box_id].mAABB.mExtents);
if(P->IsLeaf())
{
// The input tree must be complete => i.e. one primitive/leaf
ASSERT(P->GetNbPrimitives()==1);
// Get the primitive index from the input tree
udword PrimitiveIndex = P->GetPrimitives()[0];
// Setup prev box data as the primitive index, marked as leaf
linear[box_id].mPosData = (PrimitiveIndex<<1)|1;
}
else
{
// Get a new id for positive child
udword PosID = current_id++;
// Setup box data
linear[box_id].mPosData = (size_t)&linear[PosID];
// Make sure it's not marked as leaf
ASSERT(!(linear[box_id].mPosData&1));
// Recurse
_BuildNoLeafTree(linear, PosID, current_id, P);
}
if(N->IsLeaf())
{
// The input tree must be complete => i.e. one primitive/leaf
ASSERT(N->GetNbPrimitives()==1);
// Get the primitive index from the input tree
udword PrimitiveIndex = N->GetPrimitives()[0];
// Setup prev box data as the primitive index, marked as leaf
linear[box_id].mNegData = (PrimitiveIndex<<1)|1;
}
else
{
// Get a new id for negative child
udword NegID = current_id++;
// Setup box data
linear[box_id].mNegData = (size_t)&linear[NegID];
// Make sure it's not marked as leaf
ASSERT(!(linear[box_id].mNegData&1));
// Recurse
_BuildNoLeafTree(linear, NegID, current_id, N);
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Constructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
AABBCollisionTree::AABBCollisionTree() : mNodes(null)
{
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Destructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
AABBCollisionTree::~AABBCollisionTree()
{
DELETEARRAY(mNodes);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Builds the collision tree from a generic AABB tree.
* \param tree [in] generic AABB tree
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool AABBCollisionTree::Build(AABBTree* tree)
{
// Checkings
if(!tree) return false;
// Check the input tree is complete
udword NbTriangles = tree->GetNbPrimitives();
udword NbNodes = tree->GetNbNodes();
if(NbNodes!=NbTriangles*2-1) return false;
// Get nodes
if(mNbNodes!=NbNodes) // Same number of nodes => keep moving
{
mNbNodes = NbNodes;
DELETEARRAY(mNodes);
mNodes = new AABBCollisionNode[mNbNodes];
CHECKALLOC(mNodes);
}
// Build the tree
udword CurID = 1;
_BuildCollisionTree(mNodes, 0, CurID, tree);
ASSERT(CurID==mNbNodes);
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Refits the collision tree after vertices have been modified.
* \param mesh_interface [in] mesh interface for current model
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool AABBCollisionTree::Refit(const MeshInterface* mesh_interface)
{
ASSERT(!"Not implemented since AABBCollisionTrees have twice as more nodes to refit as AABBNoLeafTrees!");
return false;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Walks the tree and call the user back for each node.
* \param callback [in] walking callback
* \param user_data [in] callback's user data
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool AABBCollisionTree::Walk(GenericWalkingCallback callback, void* user_data) const
{
if(!callback) return false;
struct Local
{
static void _Walk(const AABBCollisionNode* current_node, GenericWalkingCallback callback, void* user_data)
{
if(!current_node || !(callback)(current_node, user_data)) return;
if(!current_node->IsLeaf())
{
_Walk(current_node->GetPos(), callback, user_data);
_Walk(current_node->GetNeg(), callback, user_data);
}
}
};
Local::_Walk(mNodes, callback, user_data);
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Constructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
AABBNoLeafTree::AABBNoLeafTree() : mNodes(null)
{
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Destructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
AABBNoLeafTree::~AABBNoLeafTree()
{
DELETEARRAY(mNodes);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Builds the collision tree from a generic AABB tree.
* \param tree [in] generic AABB tree
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool AABBNoLeafTree::Build(AABBTree* tree)
{
// Checkings
if(!tree) return false;
// Check the input tree is complete
udword NbTriangles = tree->GetNbPrimitives();
udword NbNodes = tree->GetNbNodes();
if(NbNodes!=NbTriangles*2-1) return false;
// Get nodes
if(mNbNodes!=NbTriangles-1) // Same number of nodes => keep moving
{
mNbNodes = NbTriangles-1;
DELETEARRAY(mNodes);
mNodes = new AABBNoLeafNode[mNbNodes];
CHECKALLOC(mNodes);
}
// Build the tree
udword CurID = 1;
_BuildNoLeafTree(mNodes, 0, CurID, tree);
ASSERT(CurID==mNbNodes);
return true;
}
inline_ void ComputeMinMax(Point& min, Point& max, const VertexPointers& vp)
{
// Compute triangle's AABB = a leaf box
#ifdef OPC_USE_FCOMI // a 15% speedup on my machine, not much
min.x = FCMin3(vp.Vertex[0]->x, vp.Vertex[1]->x, vp.Vertex[2]->x);
max.x = FCMax3(vp.Vertex[0]->x, vp.Vertex[1]->x, vp.Vertex[2]->x);
min.y = FCMin3(vp.Vertex[0]->y, vp.Vertex[1]->y, vp.Vertex[2]->y);
max.y = FCMax3(vp.Vertex[0]->y, vp.Vertex[1]->y, vp.Vertex[2]->y);
min.z = FCMin3(vp.Vertex[0]->z, vp.Vertex[1]->z, vp.Vertex[2]->z);
max.z = FCMax3(vp.Vertex[0]->z, vp.Vertex[1]->z, vp.Vertex[2]->z);
#else
min = *vp.Vertex[0];
max = *vp.Vertex[0];
min.Min(*vp.Vertex[1]);
max.Max(*vp.Vertex[1]);
min.Min(*vp.Vertex[2]);
max.Max(*vp.Vertex[2]);
#endif
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Refits the collision tree after vertices have been modified.
* \param mesh_interface [in] mesh interface for current model
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool AABBNoLeafTree::Refit(const MeshInterface* mesh_interface)
{
// Checkings
if(!mesh_interface) return false;
// Bottom-up update
VertexPointers VP;
Point Min,Max;
Point Min_,Max_;
udword Index = mNbNodes;
while(Index--)
{
AABBNoLeafNode& Current = mNodes[Index];
if(Current.HasPosLeaf())
{
mesh_interface->GetTriangle(VP, Current.GetPosPrimitive());
ComputeMinMax(Min, Max, VP);
}
else
{
const CollisionAABB& CurrentBox = Current.GetPos()->mAABB;
CurrentBox.GetMin(Min);
CurrentBox.GetMax(Max);
}
if(Current.HasNegLeaf())
{
mesh_interface->GetTriangle(VP, Current.GetNegPrimitive());
ComputeMinMax(Min_, Max_, VP);
}
else
{
const CollisionAABB& CurrentBox = Current.GetNeg()->mAABB;
CurrentBox.GetMin(Min_);
CurrentBox.GetMax(Max_);
}
#ifdef OPC_USE_FCOMI
Min.x = FCMin2(Min.x, Min_.x);
Max.x = FCMax2(Max.x, Max_.x);
Min.y = FCMin2(Min.y, Min_.y);
Max.y = FCMax2(Max.y, Max_.y);
Min.z = FCMin2(Min.z, Min_.z);
Max.z = FCMax2(Max.z, Max_.z);
#else
Min.Min(Min_);
Max.Max(Max_);
#endif
Current.mAABB.SetMinMax(Min, Max);
}
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Walks the tree and call the user back for each node.
* \param callback [in] walking callback
* \param user_data [in] callback's user data
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool AABBNoLeafTree::Walk(GenericWalkingCallback callback, void* user_data) const
{
if(!callback) return false;
struct Local
{
static void _Walk(const AABBNoLeafNode* current_node, GenericWalkingCallback callback, void* user_data)
{
if(!current_node || !(callback)(current_node, user_data)) return;
if(!current_node->HasPosLeaf()) _Walk(current_node->GetPos(), callback, user_data);
if(!current_node->HasNegLeaf()) _Walk(current_node->GetNeg(), callback, user_data);
}
};
Local::_Walk(mNodes, callback, user_data);
return true;
}
// Quantization notes:
// - We could use the highest bits of mData to store some more quantized bits. Dequantization code
// would be slightly more complex, but number of overlap tests would be reduced (and anyhow those
// bits are currently wasted). Of course it's not possible if we move to 16 bits mData.
// - Something like "16 bits floats" could be tested, to bypass the int-to-float conversion.
// - A dedicated BV-BV test could be used, dequantizing while testing for overlap. (i.e. it's some
// lazy-dequantization which may save some work in case of early exits). At the very least some
// muls could be saved by precomputing several more matrices. But maybe not worth the pain.
// - Do we need to dequantize anyway? Not doing the extents-related muls only implies the box has
// been scaled, for example.
// - The deeper we move into the hierarchy, the smaller the extents should be. May not need a fixed
// number of quantization bits. Even better, could probably be best delta-encoded.
// Find max values. Some people asked why I wasn't simply using the first node. Well, I can't.
// I'm not looking for (min, max) values like in a standard AABB, I'm looking for the extremal
// centers/extents in order to quantize them. The first node would only give a single center and
// a single extents. While extents would be the biggest, the center wouldn't.
#define FIND_MAX_VALUES \
/* Get max values */ \
Point CMax(MIN_FLOAT, MIN_FLOAT, MIN_FLOAT); \
Point EMax(MIN_FLOAT, MIN_FLOAT, MIN_FLOAT); \
for(udword i=0;i<mNbNodes;i++) \
{ \
if(fabsf(Nodes[i].mAABB.mCenter.x)>CMax.x) CMax.x = fabsf(Nodes[i].mAABB.mCenter.x); \
if(fabsf(Nodes[i].mAABB.mCenter.y)>CMax.y) CMax.y = fabsf(Nodes[i].mAABB.mCenter.y); \
if(fabsf(Nodes[i].mAABB.mCenter.z)>CMax.z) CMax.z = fabsf(Nodes[i].mAABB.mCenter.z); \
if(fabsf(Nodes[i].mAABB.mExtents.x)>EMax.x) EMax.x = fabsf(Nodes[i].mAABB.mExtents.x); \
if(fabsf(Nodes[i].mAABB.mExtents.y)>EMax.y) EMax.y = fabsf(Nodes[i].mAABB.mExtents.y); \
if(fabsf(Nodes[i].mAABB.mExtents.z)>EMax.z) EMax.z = fabsf(Nodes[i].mAABB.mExtents.z); \
}
#define INIT_QUANTIZATION \
udword nbc=15; /* Keep one bit for sign */ \
udword nbe=15; /* Keep one bit for fix */ \
if(!gFixQuantized) nbe++; \
\
/* Compute quantization coeffs */ \
Point CQuantCoeff, EQuantCoeff; \
CQuantCoeff.x = CMax.x!=0.0f ? float((1<<nbc)-1)/CMax.x : 0.0f; \
CQuantCoeff.y = CMax.y!=0.0f ? float((1<<nbc)-1)/CMax.y : 0.0f; \
CQuantCoeff.z = CMax.z!=0.0f ? float((1<<nbc)-1)/CMax.z : 0.0f; \
EQuantCoeff.x = EMax.x!=0.0f ? float((1<<nbe)-1)/EMax.x : 0.0f; \
EQuantCoeff.y = EMax.y!=0.0f ? float((1<<nbe)-1)/EMax.y : 0.0f; \
EQuantCoeff.z = EMax.z!=0.0f ? float((1<<nbe)-1)/EMax.z : 0.0f; \
/* Compute and save dequantization coeffs */ \
mCenterCoeff.x = CQuantCoeff.x!=0.0f ? 1.0f / CQuantCoeff.x : 0.0f; \
mCenterCoeff.y = CQuantCoeff.y!=0.0f ? 1.0f / CQuantCoeff.y : 0.0f; \
mCenterCoeff.z = CQuantCoeff.z!=0.0f ? 1.0f / CQuantCoeff.z : 0.0f; \
mExtentsCoeff.x = EQuantCoeff.x!=0.0f ? 1.0f / EQuantCoeff.x : 0.0f; \
mExtentsCoeff.y = EQuantCoeff.y!=0.0f ? 1.0f / EQuantCoeff.y : 0.0f; \
mExtentsCoeff.z = EQuantCoeff.z!=0.0f ? 1.0f / EQuantCoeff.z : 0.0f; \
#define PERFORM_QUANTIZATION \
/* Quantize */ \
mNodes[i].mAABB.mCenter[0] = sword(Nodes[i].mAABB.mCenter.x * CQuantCoeff.x); \
mNodes[i].mAABB.mCenter[1] = sword(Nodes[i].mAABB.mCenter.y * CQuantCoeff.y); \
mNodes[i].mAABB.mCenter[2] = sword(Nodes[i].mAABB.mCenter.z * CQuantCoeff.z); \
mNodes[i].mAABB.mExtents[0] = uword(Nodes[i].mAABB.mExtents.x * EQuantCoeff.x); \
mNodes[i].mAABB.mExtents[1] = uword(Nodes[i].mAABB.mExtents.y * EQuantCoeff.y); \
mNodes[i].mAABB.mExtents[2] = uword(Nodes[i].mAABB.mExtents.z * EQuantCoeff.z); \
/* Fix quantized boxes */ \
if(gFixQuantized) \
{ \
/* Make sure the quantized box is still valid */ \
Point Max = Nodes[i].mAABB.mCenter + Nodes[i].mAABB.mExtents; \
Point Min = Nodes[i].mAABB.mCenter - Nodes[i].mAABB.mExtents; \
/* For each axis */ \
for(udword j=0;j<3;j++) \
{ /* Dequantize the box center */ \
float qc = float(mNodes[i].mAABB.mCenter[j]) * mCenterCoeff[j]; \
bool FixMe=true; \
do \
{ /* Dequantize the box extent */ \
float qe = float(mNodes[i].mAABB.mExtents[j]) * mExtentsCoeff[j]; \
/* Compare real & dequantized values */ \
if(qc+qe<Max[j] || qc-qe>Min[j]) mNodes[i].mAABB.mExtents[j]++; \
else FixMe=false; \
/* Prevent wrapping */ \
if(!mNodes[i].mAABB.mExtents[j]) \
{ \
mNodes[i].mAABB.mExtents[j]=0xffff; \
FixMe=false; \
} \
}while(FixMe); \
} \
}
#define REMAP_DATA(member) \
/* Fix data */ \
Data = Nodes[i].member; \
if(!(Data&1)) \
{ \
/* Compute box number */ \
size_t Nb = (Data - size_t(Nodes))/Nodes[i].GetNodeSize(); \
Data = (size_t) &mNodes[Nb]; \
} \
/* ...remapped */ \
mNodes[i].member = Data;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Constructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
AABBQuantizedTree::AABBQuantizedTree() : mNodes(null)
{
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Destructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
AABBQuantizedTree::~AABBQuantizedTree()
{
DELETEARRAY(mNodes);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Builds the collision tree from a generic AABB tree.
* \param tree [in] generic AABB tree
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool AABBQuantizedTree::Build(AABBTree* tree)
{
// Checkings
if(!tree) return false;
// Check the input tree is complete
udword NbTriangles = tree->GetNbPrimitives();
udword NbNodes = tree->GetNbNodes();
if(NbNodes!=NbTriangles*2-1) return false;
// Get nodes
mNbNodes = NbNodes;
DELETEARRAY(mNodes);
AABBCollisionNode* Nodes = new AABBCollisionNode[mNbNodes];
CHECKALLOC(Nodes);
// Build the tree
udword CurID = 1;
_BuildCollisionTree(Nodes, 0, CurID, tree);
// Quantize
{
mNodes = new AABBQuantizedNode[mNbNodes];
CHECKALLOC(mNodes);
// Get max values
FIND_MAX_VALUES
// Quantization
INIT_QUANTIZATION
// Quantize
size_t Data;
for(udword i=0;i<mNbNodes;i++)
{
PERFORM_QUANTIZATION
REMAP_DATA(mData)
}
DELETEARRAY(Nodes);
}
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Refits the collision tree after vertices have been modified.
* \param mesh_interface [in] mesh interface for current model
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool AABBQuantizedTree::Refit(const MeshInterface* mesh_interface)
{
ASSERT(!"Not implemented since requantizing is painful !");
return false;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Walks the tree and call the user back for each node.
* \param callback [in] walking callback
* \param user_data [in] callback's user data
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool AABBQuantizedTree::Walk(GenericWalkingCallback callback, void* user_data) const
{
if(!callback) return false;
struct Local
{
static void _Walk(const AABBQuantizedNode* current_node, GenericWalkingCallback callback, void* user_data)
{
if(!current_node || !(callback)(current_node, user_data)) return;
if(!current_node->IsLeaf())
{
_Walk(current_node->GetPos(), callback, user_data);
_Walk(current_node->GetNeg(), callback, user_data);
}
}
};
Local::_Walk(mNodes, callback, user_data);
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Constructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
AABBQuantizedNoLeafTree::AABBQuantizedNoLeafTree() : mNodes(null)
{
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Destructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
AABBQuantizedNoLeafTree::~AABBQuantizedNoLeafTree()
{
DELETEARRAY(mNodes);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Builds the collision tree from a generic AABB tree.
* \param tree [in] generic AABB tree
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool AABBQuantizedNoLeafTree::Build(AABBTree* tree)
{
// Checkings
if(!tree) return false;
// Check the input tree is complete
udword NbTriangles = tree->GetNbPrimitives();
udword NbNodes = tree->GetNbNodes();
if(NbNodes!=NbTriangles*2-1) return false;
// Get nodes
mNbNodes = NbTriangles-1;
DELETEARRAY(mNodes);
AABBNoLeafNode* Nodes = new AABBNoLeafNode[mNbNodes];
CHECKALLOC(Nodes);
// Build the tree
udword CurID = 1;
_BuildNoLeafTree(Nodes, 0, CurID, tree);
ASSERT(CurID==mNbNodes);
// Quantize
{
mNodes = new AABBQuantizedNoLeafNode[mNbNodes];
CHECKALLOC(mNodes);
// Get max values
FIND_MAX_VALUES
// Quantization
INIT_QUANTIZATION
// Quantize
size_t Data;
for(udword i=0;i<mNbNodes;i++)
{
PERFORM_QUANTIZATION
REMAP_DATA(mPosData)
REMAP_DATA(mNegData)
}
DELETEARRAY(Nodes);
}
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Refits the collision tree after vertices have been modified.
* \param mesh_interface [in] mesh interface for current model
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool AABBQuantizedNoLeafTree::Refit(const MeshInterface* mesh_interface)
{
ASSERT(!"Not implemented since requantizing is painful !");
return false;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Walks the tree and call the user back for each node.
* \param callback [in] walking callback
* \param user_data [in] callback's user data
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool AABBQuantizedNoLeafTree::Walk(GenericWalkingCallback callback, void* user_data) const
{
if(!callback) return false;
struct Local
{
static void _Walk(const AABBQuantizedNoLeafNode* current_node, GenericWalkingCallback callback, void* user_data)
{
if(!current_node || !(callback)(current_node, user_data)) return;
if(!current_node->HasPosLeaf()) _Walk(current_node->GetPos(), callback, user_data);
if(!current_node->HasNegLeaf()) _Walk(current_node->GetNeg(), callback, user_data);
}
};
Local::_Walk(mNodes, callback, user_data);
return true;
}

206
ode/OPCODE/OPC_OptimizedTree.h Normal file
View File

@ -0,0 +1,206 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for optimized trees.
* \file OPC_OptimizedTree.h
* \author Pierre Terdiman
* \date March, 20, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __OPC_OPTIMIZEDTREE_H__
#define __OPC_OPTIMIZEDTREE_H__
//! Common interface for a node of an implicit tree
#define IMPLEMENT_IMPLICIT_NODE(base_class, volume) \
public: \
/* Constructor / Destructor */ \
inline_ base_class() : mData(0) {} \
inline_ ~base_class() {} \
/* Leaf test */ \
inline_ BOOL IsLeaf() const { return (mData&1)!=0; } \
/* Data access */ \
inline_ const base_class* GetPos() const { return (base_class*)mData; } \
inline_ const base_class* GetNeg() const { return ((base_class*)mData)+1; } \
inline_ size_t GetPrimitive() const { return (mData>>1); } \
/* Stats */ \
inline_ udword GetNodeSize() const { return SIZEOFOBJECT; } \
\
volume mAABB; \
size_t mData;
//! Common interface for a node of a no-leaf tree
#define IMPLEMENT_NOLEAF_NODE(base_class, volume) \
public: \
/* Constructor / Destructor */ \
inline_ base_class() : mPosData(0), mNegData(0) {} \
inline_ ~base_class() {} \
/* Leaf tests */ \
inline_ BOOL HasPosLeaf() const { return (mPosData&1)!=0; } \
inline_ BOOL HasNegLeaf() const { return (mNegData&1)!=0; } \
/* Data access */ \
inline_ const base_class* GetPos() const { return (base_class*)mPosData; } \
inline_ const base_class* GetNeg() const { return (base_class*)mNegData; } \
inline_ size_t GetPosPrimitive() const { return (mPosData>>1); } \
inline_ size_t GetNegPrimitive() const { return (mNegData>>1); } \
/* Stats */ \
inline_ udword GetNodeSize() const { return SIZEOFOBJECT; } \
\
volume mAABB; \
size_t mPosData; \
size_t mNegData;
class OPCODE_API AABBCollisionNode
{
IMPLEMENT_IMPLICIT_NODE(AABBCollisionNode, CollisionAABB)
inline_ float GetVolume() const { return mAABB.mExtents.x * mAABB.mExtents.y * mAABB.mExtents.z; }
inline_ float GetSize() const { return mAABB.mExtents.SquareMagnitude(); }
inline_ udword GetRadius() const
{
udword* Bits = (udword*)&mAABB.mExtents.x;
udword Max = Bits[0];
if(Bits[1]>Max) Max = Bits[1];
if(Bits[2]>Max) Max = Bits[2];
return Max;
}
// NB: using the square-magnitude or the true volume of the box, seems to yield better results
// (assuming UNC-like informed traversal methods). I borrowed this idea from PQP. The usual "size"
// otherwise, is the largest box extent. In SOLID that extent is computed on-the-fly each time it's
// needed (the best approach IMHO). In RAPID the rotation matrix is permuted so that Extent[0] is
// always the greatest, which saves looking for it at runtime. On the other hand, it yields matrices
// whose determinant is not 1, i.e. you can't encode them anymore as unit quaternions. Not a very
// good strategy.
};
class OPCODE_API AABBQuantizedNode
{
IMPLEMENT_IMPLICIT_NODE(AABBQuantizedNode, QuantizedAABB)
inline_ uword GetSize() const
{
const uword* Bits = mAABB.mExtents;
uword Max = Bits[0];
if(Bits[1]>Max) Max = Bits[1];
if(Bits[2]>Max) Max = Bits[2];
return Max;
}
// NB: for quantized nodes I don't feel like computing a square-magnitude with integers all
// over the place.......!
};
class OPCODE_API AABBNoLeafNode
{
IMPLEMENT_NOLEAF_NODE(AABBNoLeafNode, CollisionAABB)
};
class OPCODE_API AABBQuantizedNoLeafNode
{
IMPLEMENT_NOLEAF_NODE(AABBQuantizedNoLeafNode, QuantizedAABB)
};
//! Common interface for a collision tree
#define IMPLEMENT_COLLISION_TREE(base_class, node) \
public: \
/* Constructor / Destructor */ \
base_class(); \
virtual ~base_class(); \
/* Builds from a standard tree */ \
override(AABBOptimizedTree) bool Build(AABBTree* tree); \
/* Refits the tree */ \
override(AABBOptimizedTree) bool Refit(const MeshInterface* mesh_interface); \
/* Walks the tree */ \
override(AABBOptimizedTree) bool Walk(GenericWalkingCallback callback, void* user_data) const; \
/* Data access */ \
inline_ const node* GetNodes() const { return mNodes; } \
/* Stats */ \
override(AABBOptimizedTree) udword GetUsedBytes() const { return mNbNodes*sizeof(node); } \
private: \
node* mNodes;
typedef bool (*GenericWalkingCallback) (const void* current, void* user_data);
class OPCODE_API AABBOptimizedTree
{
public:
// Constructor / Destructor
AABBOptimizedTree() :
mNbNodes (0)
{}
virtual ~AABBOptimizedTree() {}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Builds the collision tree from a generic AABB tree.
* \param tree [in] generic AABB tree
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
virtual bool Build(AABBTree* tree) = 0;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Refits the collision tree after vertices have been modified.
* \param mesh_interface [in] mesh interface for current model
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
virtual bool Refit(const MeshInterface* mesh_interface) = 0;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Walks the tree and call the user back for each node.
* \param callback [in] walking callback
* \param user_data [in] callback's user data
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
virtual bool Walk(GenericWalkingCallback callback, void* user_data) const = 0;
// Data access
virtual udword GetUsedBytes() const = 0;
inline_ udword GetNbNodes() const { return mNbNodes; }
protected:
udword mNbNodes;
};
class OPCODE_API AABBCollisionTree : public AABBOptimizedTree
{
IMPLEMENT_COLLISION_TREE(AABBCollisionTree, AABBCollisionNode)
};
class OPCODE_API AABBNoLeafTree : public AABBOptimizedTree
{
IMPLEMENT_COLLISION_TREE(AABBNoLeafTree, AABBNoLeafNode)
};
class OPCODE_API AABBQuantizedTree : public AABBOptimizedTree
{
IMPLEMENT_COLLISION_TREE(AABBQuantizedTree, AABBQuantizedNode)
public:
Point mCenterCoeff;
Point mExtentsCoeff;
};
class OPCODE_API AABBQuantizedNoLeafTree : public AABBOptimizedTree
{
IMPLEMENT_COLLISION_TREE(AABBQuantizedNoLeafTree, AABBQuantizedNoLeafNode)
public:
Point mCenterCoeff;
Point mExtentsCoeff;
};
#endif // __OPC_OPTIMIZEDTREE_H__

182
ode/OPCODE/OPC_Picking.cpp Normal file
View File

@ -0,0 +1,182 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code to perform "picking".
* \file OPC_Picking.cpp
* \author Pierre Terdiman
* \date March, 20, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Precompiled Header
#include "Stdafx.h"
using namespace Opcode;
#ifdef OPC_RAYHIT_CALLBACK
/*
Possible RayCollider usages:
- boolean query (shadow feeler)
- closest hit
- all hits
- number of intersection (boolean)
*/
bool Opcode::SetupAllHits(RayCollider& collider, CollisionFaces& contacts)
{
struct Local
{
static void AllContacts(const CollisionFace& hit, void* user_data)
{
CollisionFaces* CF = (CollisionFaces*)user_data;
CF->AddFace(hit);
}
};
collider.SetFirstContact(false);
collider.SetHitCallback(Local::AllContacts);
collider.SetUserData(&contacts);
return true;
}
bool Opcode::SetupClosestHit(RayCollider& collider, CollisionFace& closest_contact)
{
struct Local
{
static void ClosestContact(const CollisionFace& hit, void* user_data)
{
CollisionFace* CF = (CollisionFace*)user_data;
if(hit.mDistance<CF->mDistance) *CF = hit;
}
};
collider.SetFirstContact(false);
collider.SetHitCallback(Local::ClosestContact);
collider.SetUserData(&closest_contact);
closest_contact.mDistance = MAX_FLOAT;
return true;
}
bool Opcode::SetupShadowFeeler(RayCollider& collider)
{
collider.SetFirstContact(true);
collider.SetHitCallback(null);
return true;
}
bool Opcode::SetupInOutTest(RayCollider& collider)
{
collider.SetFirstContact(false);
collider.SetHitCallback(null);
// Results with collider.GetNbIntersections()
return true;
}
bool Opcode::Picking(
CollisionFace& picked_face,
const Ray& world_ray, const Model& model, const Matrix4x4* world,
float min_dist, float max_dist, const Point& view_point, CullModeCallback callback, void* user_data)
{
struct Local
{
struct CullData
{
CollisionFace* Closest;
float MinLimit;
CullModeCallback Callback;
void* UserData;
Point ViewPoint;
const MeshInterface* IMesh;
};
// Called for each stabbed face
static void RenderCullingCallback(const CollisionFace& hit, void* user_data)
{
CullData* Data = (CullData*)user_data;
// Discard face if we already have a closer hit
if(hit.mDistance>=Data->Closest->mDistance) return;
// Discard face if hit point is smaller than min limit. This mainly happens when the face is in front
// of the near clip plane (or straddles it). If we keep the face nonetheless, the user can select an
// object that he may not even be able to see, which is very annoying.
if(hit.mDistance<=Data->MinLimit) return;
// This is the index of currently stabbed triangle.
udword StabbedFaceIndex = hit.mFaceID;
// We may keep it or not, depending on backface culling
bool KeepIt = true;
// Catch *render* cull mode for this face
CullMode CM = (Data->Callback)(StabbedFaceIndex, Data->UserData);
if(CM!=CULLMODE_NONE) // Don't even compute culling for double-sided triangles
{
// Compute backface culling for current face
VertexPointers VP;
Data->IMesh->GetTriangle(VP, StabbedFaceIndex);
if(VP.BackfaceCulling(Data->ViewPoint))
{
if(CM==CULLMODE_CW) KeepIt = false;
}
else
{
if(CM==CULLMODE_CCW) KeepIt = false;
}
}
if(KeepIt) *Data->Closest = hit;
}
};
RayCollider RC;
RC.SetMaxDist(max_dist);
RC.SetTemporalCoherence(false);
RC.SetCulling(false); // We need all faces since some of them can be double-sided
RC.SetFirstContact(false);
RC.SetHitCallback(Local::RenderCullingCallback);
picked_face.mFaceID = INVALID_ID;
picked_face.mDistance = MAX_FLOAT;
picked_face.mU = 0.0f;
picked_face.mV = 0.0f;
Local::CullData Data;
Data.Closest = &picked_face;
Data.MinLimit = min_dist;
Data.Callback = callback;
Data.UserData = user_data;
Data.ViewPoint = view_point;
Data.IMesh = model.GetMeshInterface();
if(world)
{
// Get matrices
Matrix4x4 InvWorld;
InvertPRMatrix(InvWorld, *world);
// Compute camera position in mesh space
Data.ViewPoint *= InvWorld;
}
RC.SetUserData(&Data);
if(RC.Collide(world_ray, model, world))
{
return picked_face.mFaceID!=INVALID_ID;
}
return false;
}
#endif

45
ode/OPCODE/OPC_Picking.h Normal file
View File

@ -0,0 +1,45 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code to perform "picking".
* \file OPC_Picking.h
* \author Pierre Terdiman
* \date March, 20, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __OPC_PICKING_H__
#define __OPC_PICKING_H__
#ifdef OPC_RAYHIT_CALLBACK
enum CullMode
{
CULLMODE_NONE = 0,
CULLMODE_CW = 1,
CULLMODE_CCW = 2
};
typedef CullMode (*CullModeCallback)(udword triangle_index, void* user_data);
OPCODE_API bool SetupAllHits (RayCollider& collider, CollisionFaces& contacts);
OPCODE_API bool SetupClosestHit (RayCollider& collider, CollisionFace& closest_contact);
OPCODE_API bool SetupShadowFeeler (RayCollider& collider);
OPCODE_API bool SetupInOutTest (RayCollider& collider);
OPCODE_API bool Picking(
CollisionFace& picked_face,
const Ray& world_ray, const Model& model, const Matrix4x4* world,
float min_dist, float max_dist, const Point& view_point, CullModeCallback callback, void* user_data);
#endif
#endif //__OPC_PICKING_H__

50
ode/OPCODE/OPC_PlanesAABBOverlap.h Normal file
View File

@ -0,0 +1,50 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Planes-AABB overlap test.
* - original code by Ville Miettinen, from Umbra/dPVS (released on the GD-Algorithms mailing list)
* - almost used "as-is", I even left the comments (hence the frustum-related notes)
*
* \param center [in] box center
* \param extents [in] box extents
* \param out_clip_mask [out] bitmask for active planes
* \param in_clip_mask [in] bitmask for active planes
* \return TRUE if boxes overlap planes
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ BOOL PlanesCollider::PlanesAABBOverlap(const Point& center, const Point& extents, udword& out_clip_mask, udword in_clip_mask)
{
// Stats
mNbVolumeBVTests++;
const Plane* p = mPlanes;
// Evaluate through all active frustum planes. We determine the relation
// between the AABB and a plane by using the concept of "near" and "far"
// vertices originally described by Zhang (and later by M<>ller). Our
// variant here uses 3 fabs ops, 6 muls, 7 adds and two floating point
// comparisons per plane. The routine early-exits if the AABB is found
// to be outside any of the planes. The loop also constructs a new output
// clip mask. Most FPUs have a native single-cycle fabsf() operation.
udword Mask = 1; // current mask index (1,2,4,8,..)
udword TmpOutClipMask = 0; // initialize output clip mask into empty.
while(Mask<=in_clip_mask) // keep looping while we have active planes left...
{
if(in_clip_mask & Mask) // if clip plane is active, process it..
{
float NP = extents.x*fabsf(p->n.x) + extents.y*fabsf(p->n.y) + extents.z*fabsf(p->n.z); // ### fabsf could be precomputed
float MP = center.x*p->n.x + center.y*p->n.y + center.z*p->n.z + p->d;
if(NP < MP) // near vertex behind the clip plane...
return FALSE; // .. so there is no intersection..
if((-NP) < MP) // near and far vertices on different sides of plane..
TmpOutClipMask |= Mask; // .. so update the clip mask...
}
Mask+=Mask; // mk = (1<<plane)
p++; // advance to next plane
}
out_clip_mask = TmpOutClipMask; // copy output value (temp used to resolve aliasing!)
return TRUE; // indicate that AABB intersects frustum
}

653
ode/OPCODE/OPC_PlanesCollider.cpp Normal file
View File

@ -0,0 +1,653 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for a planes collider.
* \file OPC_PlanesCollider.cpp
* \author Pierre Terdiman
* \date January, 1st, 2002
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains a Planes-vs-tree collider.
*
* \class PlanesCollider
* \author Pierre Terdiman
* \version 1.3
* \date January, 1st, 2002
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Precompiled Header
#include "Stdafx.h"
using namespace Opcode;
#include "OPC_PlanesAABBOverlap.h"
#include "OPC_PlanesTriOverlap.h"
#define SET_CONTACT(prim_index, flag) \
/* Set contact status */ \
mFlags |= flag; \
mTouchedPrimitives->Add(udword(prim_index));
//! Planes-triangle test
#define PLANES_PRIM(prim_index, flag) \
/* Request vertices from the app */ \
mIMesh->GetTriangle(mVP, prim_index); \
/* Perform triangle-box overlap test */ \
if(PlanesTriOverlap(clip_mask)) \
{ \
SET_CONTACT(prim_index, flag) \
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Constructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
PlanesCollider::PlanesCollider() :
mPlanes (null),
mNbPlanes (0)
{
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Destructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
PlanesCollider::~PlanesCollider()
{
DELETEARRAY(mPlanes);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Validates current settings. You should call this method after all the settings and callbacks have been defined.
* \return null if everything is ok, else a string describing the problem
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
const char* PlanesCollider::ValidateSettings()
{
if(TemporalCoherenceEnabled() && !FirstContactEnabled()) return "Temporal coherence only works with ""First contact"" mode!";
return VolumeCollider::ValidateSettings();
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Generic collision query for generic OPCODE models. After the call, access the results:
* - with GetContactStatus()
* - with GetNbTouchedPrimitives()
* - with GetTouchedPrimitives()
*
* \param cache [in/out] a planes cache
* \param planes [in] list of planes in world space
* \param nb_planes [in] number of planes
* \param model [in] Opcode model to collide with
* \param worldm [in] model's world matrix, or null
* \return true if success
* \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool PlanesCollider::Collide(PlanesCache& cache, const Plane* planes, udword nb_planes, const Model& model, const Matrix4x4* worldm)
{
// Checkings
if(!Setup(&model)) return false;
// Init collision query
if(InitQuery(cache, planes, nb_planes, worldm)) return true;
udword PlaneMask = (1<<nb_planes)-1;
if(!model.HasLeafNodes())
{
if(model.IsQuantized())
{
const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree();
// Setup dequantization coeffs
mCenterCoeff = Tree->mCenterCoeff;
mExtentsCoeff = Tree->mExtentsCoeff;
// Perform collision query
if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask);
else _Collide(Tree->GetNodes(), PlaneMask);
}
else
{
const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree();
// Perform collision query
if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask);
else _Collide(Tree->GetNodes(), PlaneMask);
}
}
else
{
if(model.IsQuantized())
{
const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree();
// Setup dequantization coeffs
mCenterCoeff = Tree->mCenterCoeff;
mExtentsCoeff = Tree->mExtentsCoeff;
// Perform collision query
if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask);
else _Collide(Tree->GetNodes(), PlaneMask);
}
else
{
const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree();
// Perform collision query
if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask);
else _Collide(Tree->GetNodes(), PlaneMask);
}
}
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Initializes a collision query :
* - reset stats & contact status
* - compute planes in model space
* - check temporal coherence
*
* \param cache [in/out] a planes cache
* \param planes [in] list of planes
* \param nb_planes [in] number of planes
* \param worldm [in] model's world matrix, or null
* \return TRUE if we can return immediately
* \warning SCALE NOT SUPPORTED. The matrix must contain rotation & translation parts only.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
BOOL PlanesCollider::InitQuery(PlanesCache& cache, const Plane* planes, udword nb_planes, const Matrix4x4* worldm)
{
// 1) Call the base method
VolumeCollider::InitQuery();
// 2) Compute planes in model space
if(nb_planes>mNbPlanes)
{
DELETEARRAY(mPlanes);
mPlanes = new Plane[nb_planes];
}
mNbPlanes = nb_planes;
if(worldm)
{
Matrix4x4 InvWorldM;
InvertPRMatrix(InvWorldM, *worldm);
// for(udword i=0;i<nb_planes;i++) mPlanes[i] = planes[i] * InvWorldM;
for(udword i=0;i<nb_planes;i++) TransformPlane(mPlanes[i], planes[i], InvWorldM);
}
else CopyMemory(mPlanes, planes, nb_planes*sizeof(Plane));
// 3) Setup destination pointer
mTouchedPrimitives = &cache.TouchedPrimitives;
// 4) Special case: 1-triangle meshes [Opcode 1.3]
if(mCurrentModel && mCurrentModel->HasSingleNode())
{
if(!SkipPrimitiveTests())
{
// We simply perform the BV-Prim overlap test each time. We assume single triangle has index 0.
mTouchedPrimitives->Reset();
// Perform overlap test between the unique triangle and the planes (and set contact status if needed)
udword clip_mask = (1<<mNbPlanes)-1;
PLANES_PRIM(udword(0), OPC_CONTACT)
// Return immediately regardless of status
return TRUE;
}
}
// 4) Check temporal coherence:
if(TemporalCoherenceEnabled())
{
// Here we use temporal coherence
// => check results from previous frame before performing the collision query
if(FirstContactEnabled())
{
// We're only interested in the first contact found => test the unique previously touched face
if(mTouchedPrimitives->GetNbEntries())
{
// Get index of previously touched face = the first entry in the array
udword PreviouslyTouchedFace = mTouchedPrimitives->GetEntry(0);
// Then reset the array:
// - if the overlap test below is successful, the index we'll get added back anyway
// - if it isn't, then the array should be reset anyway for the normal query
mTouchedPrimitives->Reset();
// Perform overlap test between the cached triangle and the planes (and set contact status if needed)
udword clip_mask = (1<<mNbPlanes)-1;
PLANES_PRIM(PreviouslyTouchedFace, OPC_TEMPORAL_CONTACT)
// Return immediately if possible
if(GetContactStatus()) return TRUE;
}
// else no face has been touched during previous query
// => we'll have to perform a normal query
}
else mTouchedPrimitives->Reset();
}
else
{
// Here we don't use temporal coherence => do a normal query
mTouchedPrimitives->Reset();
}
return FALSE;
}
#define TEST_CLIP_MASK \
/* If the box is completely included, so are its children. We don't need to do extra tests, we */ \
/* can immediately output a list of visible children. Those ones won't need to be clipped. */ \
if(!OutClipMask) \
{ \
/* Set contact status */ \
mFlags |= OPC_CONTACT; \
_Dump(node); \
return; \
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for normal AABB trees.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void PlanesCollider::_Collide(const AABBCollisionNode* node, udword clip_mask)
{
// Test the box against the planes. If the box is completely culled, so are its children, hence we exit.
udword OutClipMask;
if(!PlanesAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents, OutClipMask, clip_mask)) return;
TEST_CLIP_MASK
// Else the box straddles one or several planes, so we need to recurse down the tree.
if(node->IsLeaf())
{
PLANES_PRIM(node->GetPrimitive(), OPC_CONTACT)
}
else
{
_Collide(node->GetPos(), OutClipMask);
if(ContactFound()) return;
_Collide(node->GetNeg(), OutClipMask);
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for normal AABB trees.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void PlanesCollider::_CollideNoPrimitiveTest(const AABBCollisionNode* node, udword clip_mask)
{
// Test the box against the planes. If the box is completely culled, so are its children, hence we exit.
udword OutClipMask;
if(!PlanesAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents, OutClipMask, clip_mask)) return;
TEST_CLIP_MASK
// Else the box straddles one or several planes, so we need to recurse down the tree.
if(node->IsLeaf())
{
SET_CONTACT(node->GetPrimitive(), OPC_CONTACT)
}
else
{
_CollideNoPrimitiveTest(node->GetPos(), OutClipMask);
if(ContactFound()) return;
_CollideNoPrimitiveTest(node->GetNeg(), OutClipMask);
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for quantized AABB trees.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void PlanesCollider::_Collide(const AABBQuantizedNode* node, udword clip_mask)
{
// Dequantize box
const QuantizedAABB& Box = node->mAABB;
const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
// Test the box against the planes. If the box is completely culled, so are its children, hence we exit.
udword OutClipMask;
if(!PlanesAABBOverlap(Center, Extents, OutClipMask, clip_mask)) return;
TEST_CLIP_MASK
// Else the box straddles one or several planes, so we need to recurse down the tree.
if(node->IsLeaf())
{
PLANES_PRIM(node->GetPrimitive(), OPC_CONTACT)
}
else
{
_Collide(node->GetPos(), OutClipMask);
if(ContactFound()) return;
_Collide(node->GetNeg(), OutClipMask);
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for quantized AABB trees.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void PlanesCollider::_CollideNoPrimitiveTest(const AABBQuantizedNode* node, udword clip_mask)
{
// Dequantize box
const QuantizedAABB& Box = node->mAABB;
const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
// Test the box against the planes. If the box is completely culled, so are its children, hence we exit.
udword OutClipMask;
if(!PlanesAABBOverlap(Center, Extents, OutClipMask, clip_mask)) return;
TEST_CLIP_MASK
// Else the box straddles one or several planes, so we need to recurse down the tree.
if(node->IsLeaf())
{
SET_CONTACT(node->GetPrimitive(), OPC_CONTACT)
}
else
{
_CollideNoPrimitiveTest(node->GetPos(), OutClipMask);
if(ContactFound()) return;
_CollideNoPrimitiveTest(node->GetNeg(), OutClipMask);
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for no-leaf AABB trees.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void PlanesCollider::_Collide(const AABBNoLeafNode* node, udword clip_mask)
{
// Test the box against the planes. If the box is completely culled, so are its children, hence we exit.
udword OutClipMask;
if(!PlanesAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents, OutClipMask, clip_mask)) return;
TEST_CLIP_MASK
// Else the box straddles one or several planes, so we need to recurse down the tree.
if(node->HasPosLeaf()) { PLANES_PRIM(node->GetPosPrimitive(), OPC_CONTACT) }
else _Collide(node->GetPos(), OutClipMask);
if(ContactFound()) return;
if(node->HasNegLeaf()) { PLANES_PRIM(node->GetNegPrimitive(), OPC_CONTACT) }
else _Collide(node->GetNeg(), OutClipMask);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for no-leaf AABB trees.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void PlanesCollider::_CollideNoPrimitiveTest(const AABBNoLeafNode* node, udword clip_mask)
{
// Test the box against the planes. If the box is completely culled, so are its children, hence we exit.
udword OutClipMask;
if(!PlanesAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents, OutClipMask, clip_mask)) return;
TEST_CLIP_MASK
// Else the box straddles one or several planes, so we need to recurse down the tree.
if(node->HasPosLeaf()) { SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) }
else _CollideNoPrimitiveTest(node->GetPos(), OutClipMask);
if(ContactFound()) return;
if(node->HasNegLeaf()) { SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) }
else _CollideNoPrimitiveTest(node->GetNeg(), OutClipMask);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for quantized no-leaf AABB trees.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void PlanesCollider::_Collide(const AABBQuantizedNoLeafNode* node, udword clip_mask)
{
// Dequantize box
const QuantizedAABB& Box = node->mAABB;
const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
// Test the box against the planes. If the box is completely culled, so are its children, hence we exit.
udword OutClipMask;
if(!PlanesAABBOverlap(Center, Extents, OutClipMask, clip_mask)) return;
TEST_CLIP_MASK
// Else the box straddles one or several planes, so we need to recurse down the tree.
if(node->HasPosLeaf()) { PLANES_PRIM(node->GetPosPrimitive(), OPC_CONTACT) }
else _Collide(node->GetPos(), OutClipMask);
if(ContactFound()) return;
if(node->HasNegLeaf()) { PLANES_PRIM(node->GetNegPrimitive(), OPC_CONTACT) }
else _Collide(node->GetNeg(), OutClipMask);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for quantized no-leaf AABB trees.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void PlanesCollider::_CollideNoPrimitiveTest(const AABBQuantizedNoLeafNode* node, udword clip_mask)
{
// Dequantize box
const QuantizedAABB& Box = node->mAABB;
const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
// Test the box against the planes. If the box is completely culled, so are its children, hence we exit.
udword OutClipMask;
if(!PlanesAABBOverlap(Center, Extents, OutClipMask, clip_mask)) return;
TEST_CLIP_MASK
// Else the box straddles one or several planes, so we need to recurse down the tree.
if(node->HasPosLeaf()) { SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) }
else _CollideNoPrimitiveTest(node->GetPos(), OutClipMask);
if(ContactFound()) return;
if(node->HasNegLeaf()) { SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) }
else _CollideNoPrimitiveTest(node->GetNeg(), OutClipMask);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Constructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
HybridPlanesCollider::HybridPlanesCollider()
{
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Destructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
HybridPlanesCollider::~HybridPlanesCollider()
{
}
bool HybridPlanesCollider::Collide(PlanesCache& cache, const Plane* planes, udword nb_planes, const HybridModel& model, const Matrix4x4* worldm)
{
// We don't want primitive tests here!
mFlags |= OPC_NO_PRIMITIVE_TESTS;
// Checkings
if(!Setup(&model)) return false;
// Init collision query
if(InitQuery(cache, planes, nb_planes, worldm)) return true;
// Special case for 1-leaf trees
if(mCurrentModel && mCurrentModel->HasSingleNode())
{
// Here we're supposed to perform a normal query, except our tree has a single node, i.e. just a few triangles
udword Nb = mIMesh->GetNbTriangles();
// Loop through all triangles
udword clip_mask = (1<<mNbPlanes)-1;
for(udword i=0;i<Nb;i++)
{
PLANES_PRIM(i, OPC_CONTACT)
}
return true;
}
// Override destination array since we're only going to get leaf boxes here
mTouchedBoxes.Reset();
mTouchedPrimitives = &mTouchedBoxes;
udword PlaneMask = (1<<nb_planes)-1;
// Now, do the actual query against leaf boxes
if(!model.HasLeafNodes())
{
if(model.IsQuantized())
{
const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree();
// Setup dequantization coeffs
mCenterCoeff = Tree->mCenterCoeff;
mExtentsCoeff = Tree->mExtentsCoeff;
// Perform collision query - we don't want primitive tests here!
_CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask);
}
else
{
const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree();
// Perform collision query - we don't want primitive tests here!
_CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask);
}
}
else
{
if(model.IsQuantized())
{
const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree();
// Setup dequantization coeffs
mCenterCoeff = Tree->mCenterCoeff;
mExtentsCoeff = Tree->mExtentsCoeff;
// Perform collision query - we don't want primitive tests here!
_CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask);
}
else
{
const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree();
// Perform collision query - we don't want primitive tests here!
_CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask);
}
}
// We only have a list of boxes so far
if(GetContactStatus())
{
// Reset contact status, since it currently only reflects collisions with leaf boxes
Collider::InitQuery();
// Change dest container so that we can use built-in overlap tests and get collided primitives
cache.TouchedPrimitives.Reset();
mTouchedPrimitives = &cache.TouchedPrimitives;
// Read touched leaf boxes
udword Nb = mTouchedBoxes.GetNbEntries();
const udword* Touched = mTouchedBoxes.GetEntries();
const LeafTriangles* LT = model.GetLeafTriangles();
const udword* Indices = model.GetIndices();
// Loop through touched leaves
udword clip_mask = (1<<mNbPlanes)-1;
while(Nb--)
{
const LeafTriangles& CurrentLeaf = LT[*Touched++];
// Each leaf box has a set of triangles
udword NbTris = CurrentLeaf.GetNbTriangles();
if(Indices)
{
const udword* T = &Indices[CurrentLeaf.GetTriangleIndex()];
// Loop through triangles and test each of them
while(NbTris--)
{
udword TriangleIndex = *T++;
PLANES_PRIM(TriangleIndex, OPC_CONTACT)
}
}
else
{
udword BaseIndex = CurrentLeaf.GetTriangleIndex();
// Loop through triangles and test each of them
while(NbTris--)
{
udword TriangleIndex = BaseIndex++;
PLANES_PRIM(TriangleIndex, OPC_CONTACT)
}
}
}
}
return true;
}

121
ode/OPCODE/OPC_PlanesCollider.h Normal file
View File

@ -0,0 +1,121 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for a planes collider.
* \file OPC_PlanesCollider.h
* \author Pierre Terdiman
* \date January, 1st, 2002
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __OPC_PLANESCOLLIDER_H__
#define __OPC_PLANESCOLLIDER_H__
struct OPCODE_API PlanesCache : VolumeCache
{
PlanesCache()
{
}
};
class OPCODE_API PlanesCollider : public VolumeCollider
{
public:
// Constructor / Destructor
PlanesCollider();
virtual ~PlanesCollider();
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Generic collision query for generic OPCODE models. After the call, access the results:
* - with GetContactStatus()
* - with GetNbTouchedPrimitives()
* - with GetTouchedPrimitives()
*
* \param cache [in/out] a planes cache
* \param planes [in] list of planes in world space
* \param nb_planes [in] number of planes
* \param model [in] Opcode model to collide with
* \param worldm [in] model's world matrix, or null
* \return true if success
* \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool Collide(PlanesCache& cache, const Plane* planes, udword nb_planes, const Model& model, const Matrix4x4* worldm=null);
// Mutant box-with-planes collision queries
inline_ bool Collide(PlanesCache& cache, const OBB& box, const Model& model, const Matrix4x4* worldb=null, const Matrix4x4* worldm=null)
{
Plane PL[6];
if(worldb)
{
// Create a new OBB in world space
OBB WorldBox;
box.Rotate(*worldb, WorldBox);
// Compute planes from the sides of the box
WorldBox.ComputePlanes(PL);
}
else
{
// Compute planes from the sides of the box
box.ComputePlanes(PL);
}
// Collide with box planes
return Collide(cache, PL, 6, model, worldm);
}
// Settings
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Validates current settings. You should call this method after all the settings and callbacks have been defined for a collider.
* \return null if everything is ok, else a string describing the problem
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
override(Collider) const char* ValidateSettings();
protected:
// Planes in model space
udword mNbPlanes;
Plane* mPlanes;
// Leaf description
VertexPointers mVP;
// Internal methods
void _Collide(const AABBCollisionNode* node, udword clip_mask);
void _Collide(const AABBNoLeafNode* node, udword clip_mask);
void _Collide(const AABBQuantizedNode* node, udword clip_mask);
void _Collide(const AABBQuantizedNoLeafNode* node, udword clip_mask);
void _CollideNoPrimitiveTest(const AABBCollisionNode* node, udword clip_mask);
void _CollideNoPrimitiveTest(const AABBNoLeafNode* node, udword clip_mask);
void _CollideNoPrimitiveTest(const AABBQuantizedNode* node, udword clip_mask);
void _CollideNoPrimitiveTest(const AABBQuantizedNoLeafNode* node, udword clip_mask);
// Overlap tests
inline_ BOOL PlanesAABBOverlap(const Point& center, const Point& extents, udword& out_clip_mask, udword in_clip_mask);
inline_ BOOL PlanesTriOverlap(udword in_clip_mask);
// Init methods
BOOL InitQuery(PlanesCache& cache, const Plane* planes, udword nb_planes, const Matrix4x4* worldm=null);
};
class OPCODE_API HybridPlanesCollider : public PlanesCollider
{
public:
// Constructor / Destructor
HybridPlanesCollider();
virtual ~HybridPlanesCollider();
bool Collide(PlanesCache& cache, const Plane* planes, udword nb_planes, const HybridModel& model, const Matrix4x4* worldm=null);
protected:
Container mTouchedBoxes;
};
#endif // __OPC_PLANESCOLLIDER_H__

40
ode/OPCODE/OPC_PlanesTriOverlap.h Normal file
View File

@ -0,0 +1,40 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Planes-triangle overlap test.
* \param in_clip_mask [in] bitmask for active planes
* \return TRUE if triangle overlap planes
* \warning THIS IS A CONSERVATIVE TEST !! Some triangles will be returned as intersecting, while they're not!
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ BOOL PlanesCollider::PlanesTriOverlap(udword in_clip_mask)
{
// Stats
mNbVolumePrimTests++;
const Plane* p = mPlanes;
udword Mask = 1;
while(Mask<=in_clip_mask)
{
if(in_clip_mask & Mask)
{
float d0 = p->Distance(*mVP.Vertex[0]);
float d1 = p->Distance(*mVP.Vertex[1]);
float d2 = p->Distance(*mVP.Vertex[2]);
if(d0>0.0f && d1>0.0f && d2>0.0f) return FALSE;
// if(!(IR(d0)&SIGN_BITMASK) && !(IR(d1)&SIGN_BITMASK) && !(IR(d2)&SIGN_BITMASK)) return FALSE;
}
Mask+=Mask;
p++;
}
/*
for(udword i=0;i<6;i++)
{
float d0 = p[i].Distance(mLeafVerts[0]);
float d1 = p[i].Distance(mLeafVerts[1]);
float d2 = p[i].Distance(mLeafVerts[2]);
if(d0>0.0f && d1>0.0f && d2>0.0f) return false;
}
*/
return TRUE;
}

63
ode/OPCODE/OPC_RayAABBOverlap.h Normal file
View File

@ -0,0 +1,63 @@
// Opcode 1.1: ray-AABB overlap tests based on Woo's code
// Opcode 1.2: ray-AABB overlap tests based on the separating axis theorem
//
// The point of intersection is not computed anymore. The distance to impact is not needed anymore
// since we now have two different queries for segments or rays.
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes a segment-AABB overlap test using the separating axis theorem. Segment is cached within the class.
* \param center [in] AABB center
* \param extents [in] AABB extents
* \return true on overlap
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ BOOL RayCollider::SegmentAABBOverlap(const Point& center, const Point& extents)
{
// Stats
mNbRayBVTests++;
float Dx = mData2.x - center.x; if(fabsf(Dx) > extents.x + mFDir.x) return FALSE;
float Dy = mData2.y - center.y; if(fabsf(Dy) > extents.y + mFDir.y) return FALSE;
float Dz = mData2.z - center.z; if(fabsf(Dz) > extents.z + mFDir.z) return FALSE;
float f;
f = mData.y * Dz - mData.z * Dy; if(fabsf(f) > extents.y*mFDir.z + extents.z*mFDir.y) return FALSE;
f = mData.z * Dx - mData.x * Dz; if(fabsf(f) > extents.x*mFDir.z + extents.z*mFDir.x) return FALSE;
f = mData.x * Dy - mData.y * Dx; if(fabsf(f) > extents.x*mFDir.y + extents.y*mFDir.x) return FALSE;
return TRUE;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes a ray-AABB overlap test using the separating axis theorem. Ray is cached within the class.
* \param center [in] AABB center
* \param extents [in] AABB extents
* \return true on overlap
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ BOOL RayCollider::RayAABBOverlap(const Point& center, const Point& extents)
{
// Stats
mNbRayBVTests++;
// float Dx = mOrigin.x - center.x; if(fabsf(Dx) > extents.x && Dx*mDir.x>=0.0f) return FALSE;
// float Dy = mOrigin.y - center.y; if(fabsf(Dy) > extents.y && Dy*mDir.y>=0.0f) return FALSE;
// float Dz = mOrigin.z - center.z; if(fabsf(Dz) > extents.z && Dz*mDir.z>=0.0f) return FALSE;
float Dx = mOrigin.x - center.x; if(GREATER(Dx, extents.x) && Dx*mDir.x>=0.0f) return FALSE;
float Dy = mOrigin.y - center.y; if(GREATER(Dy, extents.y) && Dy*mDir.y>=0.0f) return FALSE;
float Dz = mOrigin.z - center.z; if(GREATER(Dz, extents.z) && Dz*mDir.z>=0.0f) return FALSE;
// float Dx = mOrigin.x - center.x; if(GREATER(Dx, extents.x) && ((SIR(Dx)-1)^SIR(mDir.x))>=0.0f) return FALSE;
// float Dy = mOrigin.y - center.y; if(GREATER(Dy, extents.y) && ((SIR(Dy)-1)^SIR(mDir.y))>=0.0f) return FALSE;
// float Dz = mOrigin.z - center.z; if(GREATER(Dz, extents.z) && ((SIR(Dz)-1)^SIR(mDir.z))>=0.0f) return FALSE;
float f;
f = mDir.y * Dz - mDir.z * Dy; if(fabsf(f) > extents.y*mFDir.z + extents.z*mFDir.y) return FALSE;
f = mDir.z * Dx - mDir.x * Dz; if(fabsf(f) > extents.x*mFDir.z + extents.z*mFDir.x) return FALSE;
f = mDir.x * Dy - mDir.y * Dx; if(fabsf(f) > extents.x*mFDir.y + extents.y*mFDir.x) return FALSE;
return TRUE;
}

762
ode/OPCODE/OPC_RayCollider.cpp Normal file
View File

@ -0,0 +1,762 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for a ray collider.
* \file OPC_RayCollider.cpp
* \author Pierre Terdiman
* \date June, 2, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains a ray-vs-tree collider.
* This class performs a stabbing query on an AABB tree, i.e. does a ray-mesh collision.
*
* HIGHER DISTANCE BOUND:
*
* If P0 and P1 are two 3D points, let's define:
* - d = distance between P0 and P1
* - Origin = P0
* - Direction = (P1 - P0) / d = normalized direction vector
* - A parameter t such as a point P on the line (P0,P1) is P = Origin + t * Direction
* - t = 0 --> P = P0
* - t = d --> P = P1
*
* Then we can define a general "ray" as:
*
* struct Ray
* {
* Point Origin;
* Point Direction;
* };
*
* But it actually maps three different things:
* - a segment, when 0 <= t <= d
* - a half-line, when 0 <= t < +infinity, or -infinity < t <= d
* - a line, when -infinity < t < +infinity
*
* In Opcode, we support segment queries, which yield half-line queries by setting d = +infinity.
* We don't support line-queries. If you need them, shift the origin along the ray by an appropriate margin.
*
* In short, the lower bound is always 0, and you can setup the higher bound "d" with RayCollider::SetMaxDist().
*
* Query |segment |half-line |line
* --------|-------------------|---------------|----------------
* Usages |-shadow feelers |-raytracing |-
* |-sweep tests |-in/out tests |
*
* FIRST CONTACT:
*
* - You can setup "first contact" mode or "all contacts" mode with RayCollider::SetFirstContact().
* - In "first contact" mode we return as soon as the ray hits one face. If can be useful e.g. for shadow feelers, where
* you want to know whether the path to the light is free or not (a boolean answer is enough).
* - In "all contacts" mode we return all faces hit by the ray.
*
* TEMPORAL COHERENCE:
*
* - You can enable or disable temporal coherence with RayCollider::SetTemporalCoherence().
* - It currently only works in "first contact" mode.
* - If temporal coherence is enabled, the previously hit triangle is cached during the first query. Then, next queries
* start by colliding the ray against the cached triangle. If they still collide, we return immediately.
*
* CLOSEST HIT:
*
* - You can enable or disable "closest hit" with RayCollider::SetClosestHit().
* - It currently only works in "all contacts" mode.
* - If closest hit is enabled, faces are sorted by distance on-the-fly and the closest one only is reported.
*
* BACKFACE CULLING:
*
* - You can enable or disable backface culling with RayCollider::SetCulling().
* - If culling is enabled, ray will not hit back faces (only front faces).
*
*
*
* \class RayCollider
* \author Pierre Terdiman
* \version 1.3
* \date June, 2, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* This class describes a face hit by a ray or segment.
* This is a particular class dedicated to stabbing queries.
*
* \class CollisionFace
* \author Pierre Terdiman
* \version 1.3
* \date March, 20, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* This class is a dedicated collection of CollisionFace.
*
* \class CollisionFaces
* \author Pierre Terdiman
* \version 1.3
* \date March, 20, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Precompiled Header
#include "Stdafx.h"
using namespace Opcode;
#include "OPC_RayAABBOverlap.h"
#include "OPC_RayTriOverlap.h"
#define SET_CONTACT(prim_index, flag) \
mNbIntersections++; \
/* Set contact status */ \
mFlags |= flag; \
/* In any case the contact has been found and recorded in mStabbedFace */ \
mStabbedFace.mFaceID = prim_index;
#ifdef OPC_RAYHIT_CALLBACK
#define HANDLE_CONTACT(prim_index, flag) \
SET_CONTACT(prim_index, flag) \
\
if(mHitCallback) (mHitCallback)(mStabbedFace, mUserData);
#define UPDATE_CACHE \
if(cache && GetContactStatus()) \
{ \
*cache = mStabbedFace.mFaceID; \
}
#else
#define HANDLE_CONTACT(prim_index, flag) \
SET_CONTACT(prim_index, flag) \
\
/* Now we can also record it in mStabbedFaces if available */ \
if(mStabbedFaces) \
{ \
/* If we want all faces or if that's the first one we hit */ \
if(!mClosestHit || !mStabbedFaces->GetNbFaces()) \
{ \
mStabbedFaces->AddFace(mStabbedFace); \
} \
else \
{ \
/* We only keep closest hit */ \
CollisionFace* Current = const_cast<CollisionFace*>(mStabbedFaces->GetFaces()); \
if(Current && mStabbedFace.mDistance<Current->mDistance) \
{ \
*Current = mStabbedFace; \
} \
} \
}
#define UPDATE_CACHE \
if(cache && GetContactStatus() && mStabbedFaces) \
{ \
const CollisionFace* Current = mStabbedFaces->GetFaces(); \
if(Current) *cache = Current->mFaceID; \
else *cache = INVALID_ID; \
}
#endif
#define SEGMENT_PRIM(prim_index, flag) \
/* Request vertices from the app */ \
VertexPointers VP; mIMesh->GetTriangle(VP, prim_index); \
\
/* Perform ray-tri overlap test and return */ \
if(RayTriOverlap(*VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2])) \
{ \
/* Intersection point is valid if dist < segment's length */ \
/* We know dist>0 so we can use integers */ \
if(IR(mStabbedFace.mDistance)<IR(mMaxDist)) \
{ \
HANDLE_CONTACT(prim_index, flag) \
} \
}
#define RAY_PRIM(prim_index, flag) \
/* Request vertices from the app */ \
VertexPointers VP; mIMesh->GetTriangle(VP, prim_index); \
\
/* Perform ray-tri overlap test and return */ \
if(RayTriOverlap(*VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2])) \
{ \
HANDLE_CONTACT(prim_index, flag) \
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Constructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
RayCollider::RayCollider() :
mNbRayBVTests (0),
mNbRayPrimTests (0),
mNbIntersections (0),
mCulling (true),
#ifdef OPC_RAYHIT_CALLBACK
mHitCallback (null),
mUserData (0),
#else
mClosestHit (false),
mStabbedFaces (null),
#endif
mMaxDist (MAX_FLOAT)
{
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Destructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
RayCollider::~RayCollider()
{
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Validates current settings. You should call this method after all the settings and callbacks have been defined.
* \return null if everything is ok, else a string describing the problem
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
const char* RayCollider::ValidateSettings()
{
if(mMaxDist<0.0f) return "Higher distance bound must be positive!";
if(TemporalCoherenceEnabled() && !FirstContactEnabled()) return "Temporal coherence only works with ""First contact"" mode!";
#ifndef OPC_RAYHIT_CALLBACK
if(mClosestHit && FirstContactEnabled()) return "Closest hit doesn't work with ""First contact"" mode!";
if(TemporalCoherenceEnabled() && mClosestHit) return "Temporal coherence can't guarantee to report closest hit!";
#endif
if(SkipPrimitiveTests()) return "SkipPrimitiveTests not possible for RayCollider ! (not implemented)";
return null;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Generic stabbing query for generic OPCODE models. After the call, access the results:
* - with GetContactStatus()
* - in the user-provided destination array
*
* \param world_ray [in] stabbing ray in world space
* \param model [in] Opcode model to collide with
* \param world [in] model's world matrix, or null
* \param cache [in] a possibly cached face index, or null
* \return true if success
* \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool RayCollider::Collide(const Ray& world_ray, const Model& model, const Matrix4x4* world, udword* cache)
{
// Checkings
if(!Setup(&model)) return false;
// Init collision query
if(InitQuery(world_ray, world, cache)) return true;
if(!model.HasLeafNodes())
{
if(model.IsQuantized())
{
const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree();
// Setup dequantization coeffs
mCenterCoeff = Tree->mCenterCoeff;
mExtentsCoeff = Tree->mExtentsCoeff;
// Perform stabbing query
if(IR(mMaxDist)!=IEEE_MAX_FLOAT) _SegmentStab(Tree->GetNodes());
else _RayStab(Tree->GetNodes());
}
else
{
const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree();
// Perform stabbing query
if(IR(mMaxDist)!=IEEE_MAX_FLOAT) _SegmentStab(Tree->GetNodes());
else _RayStab(Tree->GetNodes());
}
}
else
{
if(model.IsQuantized())
{
const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree();
// Setup dequantization coeffs
mCenterCoeff = Tree->mCenterCoeff;
mExtentsCoeff = Tree->mExtentsCoeff;
// Perform stabbing query
if(IR(mMaxDist)!=IEEE_MAX_FLOAT) _SegmentStab(Tree->GetNodes());
else _RayStab(Tree->GetNodes());
}
else
{
const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree();
// Perform stabbing query
if(IR(mMaxDist)!=IEEE_MAX_FLOAT) _SegmentStab(Tree->GetNodes());
else _RayStab(Tree->GetNodes());
}
}
// Update cache if needed
UPDATE_CACHE
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Initializes a stabbing query :
* - reset stats & contact status
* - compute ray in local space
* - check temporal coherence
*
* \param world_ray [in] stabbing ray in world space
* \param world [in] object's world matrix, or null
* \param face_id [in] index of previously stabbed triangle
* \return TRUE if we can return immediately
* \warning SCALE NOT SUPPORTED. The matrix must contain rotation & translation parts only.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
BOOL RayCollider::InitQuery(const Ray& world_ray, const Matrix4x4* world, udword* face_id)
{
// Reset stats & contact status
Collider::InitQuery();
mNbRayBVTests = 0;
mNbRayPrimTests = 0;
mNbIntersections = 0;
#ifndef OPC_RAYHIT_CALLBACK
if(mStabbedFaces) mStabbedFaces->Reset();
#endif
// Compute ray in local space
// The (Origin/Dir) form is needed for the ray-triangle test anyway (even for segment tests)
if(world)
{
Matrix3x3 InvWorld = *world;
mDir = InvWorld * world_ray.mDir;
Matrix4x4 World;
InvertPRMatrix(World, *world);
mOrigin = world_ray.mOrig * World;
}
else
{
mDir = world_ray.mDir;
mOrigin = world_ray.mOrig;
}
// 4) Special case: 1-triangle meshes [Opcode 1.3]
if(mCurrentModel && mCurrentModel->HasSingleNode())
{
// We simply perform the BV-Prim overlap test each time. We assume single triangle has index 0.
if(!SkipPrimitiveTests())
{
// Perform overlap test between the unique triangle and the ray (and set contact status if needed)
SEGMENT_PRIM(udword(0), OPC_CONTACT)
// Return immediately regardless of status
return TRUE;
}
}
// Check temporal coherence :
// Test previously colliding primitives first
if(TemporalCoherenceEnabled() && FirstContactEnabled() && face_id && *face_id!=INVALID_ID)
{
#ifdef OLD_CODE
#ifndef OPC_RAYHIT_CALLBACK
if(!mClosestHit)
#endif
{
// Request vertices from the app
VertexPointers VP;
mIMesh->GetTriangle(VP, *face_id);
// Perform ray-cached tri overlap test
if(RayTriOverlap(*VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2]))
{
// Intersection point is valid if:
// - distance is positive (else it can just be a face behind the orig point)
// - distance is smaller than a given max distance (useful for shadow feelers)
// if(mStabbedFace.mDistance>0.0f && mStabbedFace.mDistance<mMaxDist)
if(IR(mStabbedFace.mDistance)<IR(mMaxDist)) // The other test is already performed in RayTriOverlap
{
// Set contact status
mFlags |= OPC_TEMPORAL_CONTACT;
mStabbedFace.mFaceID = *face_id;
#ifndef OPC_RAYHIT_CALLBACK
if(mStabbedFaces) mStabbedFaces->AddFace(mStabbedFace);
#endif
return TRUE;
}
}
}
#else
// New code
// We handle both Segment/ray queries with the same segment code, and a possible infinite limit
SEGMENT_PRIM(*face_id, OPC_TEMPORAL_CONTACT)
// Return immediately if possible
if(GetContactStatus()) return TRUE;
#endif
}
// Precompute data (moved after temporal coherence since only needed for ray-AABB)
if(IR(mMaxDist)!=IEEE_MAX_FLOAT)
{
// For Segment-AABB overlap
mData = 0.5f * mDir * mMaxDist;
mData2 = mOrigin + mData;
// Precompute mFDir;
mFDir.x = fabsf(mData.x);
mFDir.y = fabsf(mData.y);
mFDir.z = fabsf(mData.z);
}
else
{
// For Ray-AABB overlap
// udword x = SIR(mDir.x)-1;
// udword y = SIR(mDir.y)-1;
// udword z = SIR(mDir.z)-1;
// mData.x = FR(x);
// mData.y = FR(y);
// mData.z = FR(z);
// Precompute mFDir;
mFDir.x = fabsf(mDir.x);
mFDir.y = fabsf(mDir.y);
mFDir.z = fabsf(mDir.z);
}
return FALSE;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Stabbing query for vanilla AABB trees.
* \param world_ray [in] stabbing ray in world space
* \param tree [in] AABB tree
* \param box_indices [out] indices of stabbed boxes
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool RayCollider::Collide(const Ray& world_ray, const AABBTree* tree, Container& box_indices)
{
// ### bad design here
// This is typically called for a scene tree, full of -AABBs-, not full of triangles.
// So we don't really have "primitives" to deal with. Hence it doesn't work with
// "FirstContact" + "TemporalCoherence".
ASSERT( !(FirstContactEnabled() && TemporalCoherenceEnabled()) );
// Checkings
if(!tree) return false;
// Init collision query
// Basically this is only called to initialize precomputed data
if(InitQuery(world_ray)) return true;
// Perform stabbing query
if(IR(mMaxDist)!=IEEE_MAX_FLOAT) _SegmentStab(tree, box_indices);
else _RayStab(tree, box_indices);
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive stabbing query for normal AABB trees.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void RayCollider::_SegmentStab(const AABBCollisionNode* node)
{
// Perform Segment-AABB overlap test
if(!SegmentAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents)) return;
if(node->IsLeaf())
{
SEGMENT_PRIM(node->GetPrimitive(), OPC_CONTACT)
}
else
{
_SegmentStab(node->GetPos());
if(ContactFound()) return;
_SegmentStab(node->GetNeg());
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive stabbing query for quantized AABB trees.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void RayCollider::_SegmentStab(const AABBQuantizedNode* node)
{
// Dequantize box
const QuantizedAABB& Box = node->mAABB;
const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
// Perform Segment-AABB overlap test
if(!SegmentAABBOverlap(Center, Extents)) return;
if(node->IsLeaf())
{
SEGMENT_PRIM(node->GetPrimitive(), OPC_CONTACT)
}
else
{
_SegmentStab(node->GetPos());
if(ContactFound()) return;
_SegmentStab(node->GetNeg());
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive stabbing query for no-leaf AABB trees.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void RayCollider::_SegmentStab(const AABBNoLeafNode* node)
{
// Perform Segment-AABB overlap test
if(!SegmentAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents)) return;
if(node->HasPosLeaf())
{
SEGMENT_PRIM(node->GetPosPrimitive(), OPC_CONTACT)
}
else _SegmentStab(node->GetPos());
if(ContactFound()) return;
if(node->HasNegLeaf())
{
SEGMENT_PRIM(node->GetNegPrimitive(), OPC_CONTACT)
}
else _SegmentStab(node->GetNeg());
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive stabbing query for quantized no-leaf AABB trees.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void RayCollider::_SegmentStab(const AABBQuantizedNoLeafNode* node)
{
// Dequantize box
const QuantizedAABB& Box = node->mAABB;
const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
// Perform Segment-AABB overlap test
if(!SegmentAABBOverlap(Center, Extents)) return;
if(node->HasPosLeaf())
{
SEGMENT_PRIM(node->GetPosPrimitive(), OPC_CONTACT)
}
else _SegmentStab(node->GetPos());
if(ContactFound()) return;
if(node->HasNegLeaf())
{
SEGMENT_PRIM(node->GetNegPrimitive(), OPC_CONTACT)
}
else _SegmentStab(node->GetNeg());
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive stabbing query for vanilla AABB trees.
* \param node [in] current collision node
* \param box_indices [out] indices of stabbed boxes
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void RayCollider::_SegmentStab(const AABBTreeNode* node, Container& box_indices)
{
// Test the box against the segment
Point Center, Extents;
node->GetAABB()->GetCenter(Center);
node->GetAABB()->GetExtents(Extents);
if(!SegmentAABBOverlap(Center, Extents)) return;
if(node->IsLeaf())
{
box_indices.Add(node->GetPrimitives(), node->GetNbPrimitives());
}
else
{
_SegmentStab(node->GetPos(), box_indices);
_SegmentStab(node->GetNeg(), box_indices);
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive stabbing query for normal AABB trees.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void RayCollider::_RayStab(const AABBCollisionNode* node)
{
// Perform Ray-AABB overlap test
if(!RayAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents)) return;
if(node->IsLeaf())
{
RAY_PRIM(node->GetPrimitive(), OPC_CONTACT)
}
else
{
_RayStab(node->GetPos());
if(ContactFound()) return;
_RayStab(node->GetNeg());
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive stabbing query for quantized AABB trees.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void RayCollider::_RayStab(const AABBQuantizedNode* node)
{
// Dequantize box
const QuantizedAABB& Box = node->mAABB;
const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
// Perform Ray-AABB overlap test
if(!RayAABBOverlap(Center, Extents)) return;
if(node->IsLeaf())
{
RAY_PRIM(node->GetPrimitive(), OPC_CONTACT)
}
else
{
_RayStab(node->GetPos());
if(ContactFound()) return;
_RayStab(node->GetNeg());
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive stabbing query for no-leaf AABB trees.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void RayCollider::_RayStab(const AABBNoLeafNode* node)
{
// Perform Ray-AABB overlap test
if(!RayAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents)) return;
if(node->HasPosLeaf())
{
RAY_PRIM(node->GetPosPrimitive(), OPC_CONTACT)
}
else _RayStab(node->GetPos());
if(ContactFound()) return;
if(node->HasNegLeaf())
{
RAY_PRIM(node->GetNegPrimitive(), OPC_CONTACT)
}
else _RayStab(node->GetNeg());
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive stabbing query for quantized no-leaf AABB trees.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void RayCollider::_RayStab(const AABBQuantizedNoLeafNode* node)
{
// Dequantize box
const QuantizedAABB& Box = node->mAABB;
const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
// Perform Ray-AABB overlap test
if(!RayAABBOverlap(Center, Extents)) return;
if(node->HasPosLeaf())
{
RAY_PRIM(node->GetPosPrimitive(), OPC_CONTACT)
}
else _RayStab(node->GetPos());
if(ContactFound()) return;
if(node->HasNegLeaf())
{
RAY_PRIM(node->GetNegPrimitive(), OPC_CONTACT)
}
else _RayStab(node->GetNeg());
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive stabbing query for vanilla AABB trees.
* \param node [in] current collision node
* \param box_indices [out] indices of stabbed boxes
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void RayCollider::_RayStab(const AABBTreeNode* node, Container& box_indices)
{
// Test the box against the ray
Point Center, Extents;
node->GetAABB()->GetCenter(Center);
node->GetAABB()->GetExtents(Extents);
if(!RayAABBOverlap(Center, Extents)) return;
if(node->IsLeaf())
{
mFlags |= OPC_CONTACT;
box_indices.Add(node->GetPrimitives(), node->GetNbPrimitives());
}
else
{
_RayStab(node->GetPos(), box_indices);
_RayStab(node->GetNeg(), box_indices);
}
}

225
ode/OPCODE/OPC_RayCollider.h Normal file
View File

@ -0,0 +1,225 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for a ray collider.
* \file OPC_RayCollider.h
* \author Pierre Terdiman
* \date June, 2, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __OPC_RAYCOLLIDER_H__
#define __OPC_RAYCOLLIDER_H__
class OPCODE_API CollisionFace
{
public:
//! Constructor
inline_ CollisionFace() {}
//! Destructor
inline_ ~CollisionFace() {}
udword mFaceID; //!< Index of touched face
float mDistance; //!< Distance from collider to hitpoint
float mU, mV; //!< Impact barycentric coordinates
};
class OPCODE_API CollisionFaces : public Container
{
public:
//! Constructor
CollisionFaces() {}
//! Destructor
~CollisionFaces() {}
inline_ udword GetNbFaces() const { return GetNbEntries()>>2; }
inline_ const CollisionFace* GetFaces() const { return (const CollisionFace*)GetEntries(); }
inline_ void Reset() { Container::Reset(); }
inline_ void AddFace(const CollisionFace& face) { Add(face.mFaceID).Add(face.mDistance).Add(face.mU).Add(face.mV); }
};
#ifdef OPC_RAYHIT_CALLBACK
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* User-callback, called by OPCODE to record a hit.
* \param hit [in] current hit
* \param user_data [in] user-defined data from SetCallback()
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
typedef void (*HitCallback) (const CollisionFace& hit, void* user_data);
#endif
class OPCODE_API RayCollider : public Collider
{
public:
// Constructor / Destructor
RayCollider();
virtual ~RayCollider();
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Generic stabbing query for generic OPCODE models. After the call, access the results:
* - with GetContactStatus()
* - in the user-provided destination array
*
* \param world_ray [in] stabbing ray in world space
* \param model [in] Opcode model to collide with
* \param world [in] model's world matrix, or null
* \param cache [in] a possibly cached face index, or null
* \return true if success
* \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool Collide(const Ray& world_ray, const Model& model, const Matrix4x4* world=null, udword* cache=null);
//
bool Collide(const Ray& world_ray, const AABBTree* tree, Container& box_indices);
// Settings
#ifndef OPC_RAYHIT_CALLBACK
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Settings: enable or disable "closest hit" mode.
* \param flag [in] true to report closest hit only
* \see SetCulling(bool flag)
* \see SetMaxDist(float max_dist)
* \see SetDestination(StabbedFaces* sf)
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ void SetClosestHit(bool flag) { mClosestHit = flag; }
#endif
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Settings: enable or disable backface culling.
* \param flag [in] true to enable backface culling
* \see SetClosestHit(bool flag)
* \see SetMaxDist(float max_dist)
* \see SetDestination(StabbedFaces* sf)
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ void SetCulling(bool flag) { mCulling = flag; }
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Settings: sets the higher distance bound.
* \param max_dist [in] higher distance bound. Default = maximal value, for ray queries (else segment)
* \see SetClosestHit(bool flag)
* \see SetCulling(bool flag)
* \see SetDestination(StabbedFaces* sf)
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ void SetMaxDist(float max_dist=MAX_FLOAT) { mMaxDist = max_dist; }
#ifdef OPC_RAYHIT_CALLBACK
inline_ void SetHitCallback(HitCallback cb) { mHitCallback = cb; }
inline_ void SetUserData(void* user_data) { mUserData = user_data; }
#else
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Settings: sets the destination array for stabbed faces.
* \param cf [in] destination array, filled during queries
* \see SetClosestHit(bool flag)
* \see SetCulling(bool flag)
* \see SetMaxDist(float max_dist)
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ void SetDestination(CollisionFaces* cf) { mStabbedFaces = cf; }
#endif
// Stats
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Stats: gets the number of Ray-BV overlap tests after a collision query.
* \see GetNbRayPrimTests()
* \see GetNbIntersections()
* \return the number of Ray-BV tests performed during last query
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ udword GetNbRayBVTests() const { return mNbRayBVTests; }
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Stats: gets the number of Ray-Triangle overlap tests after a collision query.
* \see GetNbRayBVTests()
* \see GetNbIntersections()
* \return the number of Ray-Triangle tests performed during last query
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ udword GetNbRayPrimTests() const { return mNbRayPrimTests; }
// In-out test
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Stats: gets the number of intersection found after a collision query. Can be used for in/out tests.
* \see GetNbRayBVTests()
* \see GetNbRayPrimTests()
* \return the number of valid intersections during last query
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ udword GetNbIntersections() const { return mNbIntersections; }
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Validates current settings. You should call this method after all the settings and callbacks have been defined for a collider.
* \return null if everything is ok, else a string describing the problem
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
override(Collider) const char* ValidateSettings();
protected:
// Ray in local space
Point mOrigin; //!< Ray origin
Point mDir; //!< Ray direction (normalized)
Point mFDir; //!< fabsf(mDir)
Point mData, mData2;
// Stabbed faces
CollisionFace mStabbedFace; //!< Current stabbed face
#ifdef OPC_RAYHIT_CALLBACK
HitCallback mHitCallback; //!< Callback used to record a hit
void* mUserData; //!< User-defined data
#else
CollisionFaces* mStabbedFaces; //!< List of stabbed faces
#endif
// Stats
udword mNbRayBVTests; //!< Number of Ray-BV tests
udword mNbRayPrimTests; //!< Number of Ray-Primitive tests
// In-out test
udword mNbIntersections; //!< Number of valid intersections
// Dequantization coeffs
Point mCenterCoeff;
Point mExtentsCoeff;
// Settings
float mMaxDist; //!< Valid segment on the ray
#ifndef OPC_RAYHIT_CALLBACK
bool mClosestHit; //!< Report closest hit only
#endif
bool mCulling; //!< Stab culled faces or not
// Internal methods
void _SegmentStab(const AABBCollisionNode* node);
void _SegmentStab(const AABBNoLeafNode* node);
void _SegmentStab(const AABBQuantizedNode* node);
void _SegmentStab(const AABBQuantizedNoLeafNode* node);
void _SegmentStab(const AABBTreeNode* node, Container& box_indices);
void _RayStab(const AABBCollisionNode* node);
void _RayStab(const AABBNoLeafNode* node);
void _RayStab(const AABBQuantizedNode* node);
void _RayStab(const AABBQuantizedNoLeafNode* node);
void _RayStab(const AABBTreeNode* node, Container& box_indices);
// Overlap tests
inline_ BOOL RayAABBOverlap(const Point& center, const Point& extents);
inline_ BOOL SegmentAABBOverlap(const Point& center, const Point& extents);
inline_ BOOL RayTriOverlap(const Point& vert0, const Point& vert1, const Point& vert2);
// Init methods
BOOL InitQuery(const Ray& world_ray, const Matrix4x4* world=null, udword* face_id=null);
};
#endif // __OPC_RAYCOLLIDER_H__

89
ode/OPCODE/OPC_RayTriOverlap.h Normal file
View File

@ -0,0 +1,89 @@
#define LOCAL_EPSILON 0.000001f
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes a ray-triangle intersection test.
* Original code from Tomas M<>ller's "Fast Minimum Storage Ray-Triangle Intersection".
* It's been optimized a bit with integer code, and modified to return a non-intersection if distance from
* ray origin to triangle is negative.
*
* \param vert0 [in] triangle vertex
* \param vert1 [in] triangle vertex
* \param vert2 [in] triangle vertex
* \return true on overlap. mStabbedFace is filled with relevant info.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ BOOL RayCollider::RayTriOverlap(const Point& vert0, const Point& vert1, const Point& vert2)
{
// Stats
mNbRayPrimTests++;
// Find vectors for two edges sharing vert0
Point edge1 = vert1 - vert0;
Point edge2 = vert2 - vert0;
// Begin calculating determinant - also used to calculate U parameter
Point pvec = mDir^edge2;
// If determinant is near zero, ray lies in plane of triangle
float det = edge1|pvec;
if(mCulling)
{
if(det<LOCAL_EPSILON) return FALSE;
// From here, det is > 0. So we can use integer cmp.
// Calculate distance from vert0 to ray origin
Point tvec = mOrigin - vert0;
// Calculate U parameter and test bounds
mStabbedFace.mU = tvec|pvec;
// if(IR(u)&0x80000000 || u>det) return FALSE;
if(IS_NEGATIVE_FLOAT(mStabbedFace.mU) || IR(mStabbedFace.mU)>IR(det)) return FALSE;
// Prepare to test V parameter
Point qvec = tvec^edge1;
// Calculate V parameter and test bounds
mStabbedFace.mV = mDir|qvec;
if(IS_NEGATIVE_FLOAT(mStabbedFace.mV) || mStabbedFace.mU+mStabbedFace.mV>det) return FALSE;
// Calculate t, scale parameters, ray intersects triangle
mStabbedFace.mDistance = edge2|qvec;
// Det > 0 so we can early exit here
// Intersection point is valid if distance is positive (else it can just be a face behind the orig point)
if(IS_NEGATIVE_FLOAT(mStabbedFace.mDistance)) return FALSE;
// Else go on
float OneOverDet = 1.0f / det;
mStabbedFace.mDistance *= OneOverDet;
mStabbedFace.mU *= OneOverDet;
mStabbedFace.mV *= OneOverDet;
}
else
{
// the non-culling branch
if(det>-LOCAL_EPSILON && det<LOCAL_EPSILON) return FALSE;
float OneOverDet = 1.0f / det;
// Calculate distance from vert0 to ray origin
Point tvec = mOrigin - vert0;
// Calculate U parameter and test bounds
mStabbedFace.mU = (tvec|pvec) * OneOverDet;
// if(IR(u)&0x80000000 || u>1.0f) return FALSE;
if(IS_NEGATIVE_FLOAT(mStabbedFace.mU) || IR(mStabbedFace.mU)>IEEE_1_0) return FALSE;
// prepare to test V parameter
Point qvec = tvec^edge1;
// Calculate V parameter and test bounds
mStabbedFace.mV = (mDir|qvec) * OneOverDet;
if(IS_NEGATIVE_FLOAT(mStabbedFace.mV) || mStabbedFace.mU+mStabbedFace.mV>1.0f) return FALSE;
// Calculate t, ray intersects triangle
mStabbedFace.mDistance = (edge2|qvec) * OneOverDet;
// Intersection point is valid if distance is positive (else it can just be a face behind the orig point)
if(IS_NEGATIVE_FLOAT(mStabbedFace.mDistance)) return FALSE;
}
return TRUE;
}

49
ode/OPCODE/OPC_Settings.h Normal file
View File

@ -0,0 +1,49 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains compilation flags.
* \file OPC_Settings.h
* \author Pierre Terdiman
* \date May, 12, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __OPC_SETTINGS_H__
#define __OPC_SETTINGS_H__
//! Use CPU comparisons (comment that line to use standard FPU compares)
#define OPC_CPU_COMPARE
//! Use FCOMI / FCMOV on Pentium-Pro based processors (comment that line to use plain C++)
#define OPC_USE_FCOMI
//! Use epsilon value in tri-tri overlap test
#define OPC_TRITRI_EPSILON_TEST
//! Use tree-coherence or not [not implemented yet]
// #define OPC_USE_TREE_COHERENCE
//! Use callbacks or direct pointers. Using callbacks might be a bit slower (but probably not much)
// #define OPC_USE_CALLBACKS
//! Support triangle and vertex strides or not. Using strides might be a bit slower (but probably not much)
#define OPC_USE_STRIDE
//! Discard negative pointer in vanilla trees
#define OPC_NO_NEG_VANILLA_TREE
//! Use a callback in the ray collider
//#define OPC_RAYHIT_CALLBACK
// NB: no compilation flag to enable/disable stats since they're actually needed in the box/box overlap test
#endif //__OPC_SETTINGS_H__

128
ode/OPCODE/OPC_SphereAABBOverlap.h Normal file
View File

@ -0,0 +1,128 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Sphere-AABB overlap test, based on Jim Arvo's code.
* \param center [in] box center
* \param extents [in] box extents
* \return TRUE on overlap
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ BOOL SphereCollider::SphereAABBOverlap(const Point& center, const Point& extents)
{
// Stats
mNbVolumeBVTests++;
float d = 0.0f;
//find the square of the distance
//from the sphere to the box
#ifdef OLDIES
for(udword i=0;i<3;i++)
{
float tmp = mCenter[i] - center[i];
float s = tmp + extents[i];
if(s<0.0f) d += s*s;
else
{
s = tmp - extents[i];
if(s>0.0f) d += s*s;
}
}
#endif
//#ifdef NEW_TEST
// float tmp = mCenter.x - center.x;
// float s = tmp + extents.x;
float tmp,s;
tmp = mCenter.x - center.x;
s = tmp + extents.x;
if(s<0.0f)
{
d += s*s;
if(d>mRadius2) return FALSE;
}
else
{
s = tmp - extents.x;
if(s>0.0f)
{
d += s*s;
if(d>mRadius2) return FALSE;
}
}
tmp = mCenter.y - center.y;
s = tmp + extents.y;
if(s<0.0f)
{
d += s*s;
if(d>mRadius2) return FALSE;
}
else
{
s = tmp - extents.y;
if(s>0.0f)
{
d += s*s;
if(d>mRadius2) return FALSE;
}
}
tmp = mCenter.z - center.z;
s = tmp + extents.z;
if(s<0.0f)
{
d += s*s;
if(d>mRadius2) return FALSE;
}
else
{
s = tmp - extents.z;
if(s>0.0f)
{
d += s*s;
if(d>mRadius2) return FALSE;
}
}
//#endif
#ifdef OLDIES
// Point Min = center - extents;
// Point Max = center + extents;
float d = 0.0f;
//find the square of the distance
//from the sphere to the box
for(udword i=0;i<3;i++)
{
float Min = center[i] - extents[i];
// if(mCenter[i]<Min[i])
if(mCenter[i]<Min)
{
// float s = mCenter[i] - Min[i];
float s = mCenter[i] - Min;
d += s*s;
}
else
{
float Max = center[i] + extents[i];
// if(mCenter[i]>Max[i])
if(mCenter[i]>Max)
{
float s = mCenter[i] - Max;
d += s*s;
}
}
}
#endif
return d <= mRadius2;
}

739
ode/OPCODE/OPC_SphereCollider.cpp Normal file
View File

@ -0,0 +1,739 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for a sphere collider.
* \file OPC_SphereCollider.cpp
* \author Pierre Terdiman
* \date June, 2, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains a sphere-vs-tree collider.
* This class performs a collision test between a sphere and an AABB tree. You can use this to do a standard player vs world collision,
* in a Nettle/Telemachos way. It doesn't suffer from all reported bugs in those two classic codes - the "new" one by Paul Nettle is a
* debuggued version I think. Collision response can be driven by reported collision data - it works extremely well for me. In sake of
* efficiency, all meshes (that is, all AABB trees) should of course also be kept in an extra hierarchical structure (octree, whatever).
*
* \class SphereCollider
* \author Pierre Terdiman
* \version 1.3
* \date June, 2, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Precompiled Header
#include "Stdafx.h"
using namespace Opcode;
#include "OPC_SphereAABBOverlap.h"
#include "OPC_SphereTriOverlap.h"
#define SET_CONTACT(prim_index, flag) \
/* Set contact status */ \
mFlags |= flag; \
mTouchedPrimitives->Add(udword(prim_index));
//! Sphere-triangle overlap test
#define SPHERE_PRIM(prim_index, flag) \
/* Request vertices from the app */ \
VertexPointers VP; mIMesh->GetTriangle(VP, prim_index); \
\
/* Perform sphere-tri overlap test */ \
if(SphereTriOverlap(*VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2])) \
{ \
SET_CONTACT(prim_index, flag) \
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Constructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
SphereCollider::SphereCollider()
{
mCenter.Zero();
mRadius2 = 0.0f;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Destructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
SphereCollider::~SphereCollider()
{
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Generic collision query for generic OPCODE models. After the call, access the results:
* - with GetContactStatus()
* - with GetNbTouchedPrimitives()
* - with GetTouchedPrimitives()
*
* \param cache [in/out] a sphere cache
* \param sphere [in] collision sphere in local space
* \param model [in] Opcode model to collide with
* \param worlds [in] sphere's world matrix, or null
* \param worldm [in] model's world matrix, or null
* \return true if success
* \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool SphereCollider::Collide(SphereCache& cache, const Sphere& sphere, const Model& model, const Matrix4x4* worlds, const Matrix4x4* worldm)
{
// Checkings
if(!Setup(&model)) return false;
// Init collision query
if(InitQuery(cache, sphere, worlds, worldm)) return true;
// Special case for 1-leaf trees
if(mCurrentModel && mCurrentModel->HasSingleNode())
{
// Here we're supposed to perform a normal query, except our tree has a single node, i.e. just a few triangles
udword Nb = mIMesh->GetNbTriangles();
// Loop through all triangles
for(udword i=0;i<Nb;i++)
{
SPHERE_PRIM(i, OPC_CONTACT)
}
return true;
}
if(!model.HasLeafNodes())
{
if(model.IsQuantized())
{
const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree();
// Setup dequantization coeffs
mCenterCoeff = Tree->mCenterCoeff;
mExtentsCoeff = Tree->mExtentsCoeff;
// Perform collision query
if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes());
else _Collide(Tree->GetNodes());
}
else
{
const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree();
// Perform collision query
if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes());
else _Collide(Tree->GetNodes());
}
}
else
{
if(model.IsQuantized())
{
const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree();
// Setup dequantization coeffs
mCenterCoeff = Tree->mCenterCoeff;
mExtentsCoeff = Tree->mExtentsCoeff;
// Perform collision query
if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes());
else _Collide(Tree->GetNodes());
}
else
{
const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree();
// Perform collision query
if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes());
else _Collide(Tree->GetNodes());
}
}
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Initializes a collision query :
* - reset stats & contact status
* - setup matrices
* - check temporal coherence
*
* \param cache [in/out] a sphere cache
* \param sphere [in] sphere in local space
* \param worlds [in] sphere's world matrix, or null
* \param worldm [in] model's world matrix, or null
* \return TRUE if we can return immediately
* \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
BOOL SphereCollider::InitQuery(SphereCache& cache, const Sphere& sphere, const Matrix4x4* worlds, const Matrix4x4* worldm)
{
// 1) Call the base method
VolumeCollider::InitQuery();
// 2) Compute sphere in model space:
// - Precompute R^2
mRadius2 = sphere.mRadius * sphere.mRadius;
// - Compute center position
mCenter = sphere.mCenter;
// -> to world space
if(worlds) mCenter *= *worlds;
// -> to model space
if(worldm)
{
// Invert model matrix
Matrix4x4 InvWorldM;
InvertPRMatrix(InvWorldM, *worldm);
mCenter *= InvWorldM;
}
// 3) Setup destination pointer
mTouchedPrimitives = &cache.TouchedPrimitives;
// 4) Special case: 1-triangle meshes [Opcode 1.3]
if(mCurrentModel && mCurrentModel->HasSingleNode())
{
if(!SkipPrimitiveTests())
{
// We simply perform the BV-Prim overlap test each time. We assume single triangle has index 0.
mTouchedPrimitives->Reset();
// Perform overlap test between the unique triangle and the sphere (and set contact status if needed)
SPHERE_PRIM(udword(0), OPC_CONTACT)
// Return immediately regardless of status
return TRUE;
}
}
// 5) Check temporal coherence :
if(TemporalCoherenceEnabled())
{
// Here we use temporal coherence
// => check results from previous frame before performing the collision query
if(FirstContactEnabled())
{
// We're only interested in the first contact found => test the unique previously touched face
if(mTouchedPrimitives->GetNbEntries())
{
// Get index of previously touched face = the first entry in the array
udword PreviouslyTouchedFace = mTouchedPrimitives->GetEntry(0);
// Then reset the array:
// - if the overlap test below is successful, the index we'll get added back anyway
// - if it isn't, then the array should be reset anyway for the normal query
mTouchedPrimitives->Reset();
// Perform overlap test between the cached triangle and the sphere (and set contact status if needed)
SPHERE_PRIM(PreviouslyTouchedFace, OPC_TEMPORAL_CONTACT)
// Return immediately if possible
if(GetContactStatus()) return TRUE;
}
// else no face has been touched during previous query
// => we'll have to perform a normal query
}
else
{
// We're interested in all contacts =>test the new real sphere N(ew) against the previous fat sphere P(revious):
float r = sqrtf(cache.FatRadius2) - sphere.mRadius;
if(IsCacheValid(cache) && cache.Center.SquareDistance(mCenter) < r*r)
{
// - if N is included in P, return previous list
// => we simply leave the list (mTouchedFaces) unchanged
// Set contact status if needed
if(mTouchedPrimitives->GetNbEntries()) mFlags |= OPC_TEMPORAL_CONTACT;
// In any case we don't need to do a query
return TRUE;
}
else
{
// - else do the query using a fat N
// Reset cache since we'll about to perform a real query
mTouchedPrimitives->Reset();
// Make a fat sphere so that coherence will work for subsequent frames
mRadius2 *= cache.FatCoeff;
// mRadius2 = (sphere.mRadius * cache.FatCoeff)*(sphere.mRadius * cache.FatCoeff);
// Update cache with query data (signature for cached faces)
cache.Center = mCenter;
cache.FatRadius2 = mRadius2;
}
}
}
else
{
// Here we don't use temporal coherence => do a normal query
mTouchedPrimitives->Reset();
}
return FALSE;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Collision query for vanilla AABB trees.
* \param cache [in/out] a sphere cache
* \param sphere [in] collision sphere in world space
* \param tree [in] AABB tree
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool SphereCollider::Collide(SphereCache& cache, const Sphere& sphere, const AABBTree* tree)
{
// This is typically called for a scene tree, full of -AABBs-, not full of triangles.
// So we don't really have "primitives" to deal with. Hence it doesn't work with
// "FirstContact" + "TemporalCoherence".
ASSERT( !(FirstContactEnabled() && TemporalCoherenceEnabled()) );
// Checkings
if(!tree) return false;
// Init collision query
if(InitQuery(cache, sphere)) return true;
// Perform collision query
_Collide(tree);
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Checks the sphere completely contains the box. In which case we can end the query sooner.
* \param bc [in] box center
* \param be [in] box extents
* \return true if the sphere contains the whole box
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ BOOL SphereCollider::SphereContainsBox(const Point& bc, const Point& be)
{
// I assume if all 8 box vertices are inside the sphere, so does the whole box.
// Sounds ok but maybe there's a better way?
Point p;
p.x=bc.x+be.x; p.y=bc.y+be.y; p.z=bc.z+be.z; if(mCenter.SquareDistance(p)>=mRadius2) return FALSE;
p.x=bc.x-be.x; if(mCenter.SquareDistance(p)>=mRadius2) return FALSE;
p.x=bc.x+be.x; p.y=bc.y-be.y; if(mCenter.SquareDistance(p)>=mRadius2) return FALSE;
p.x=bc.x-be.x; if(mCenter.SquareDistance(p)>=mRadius2) return FALSE;
p.x=bc.x+be.x; p.y=bc.y+be.y; p.z=bc.z-be.z; if(mCenter.SquareDistance(p)>=mRadius2) return FALSE;
p.x=bc.x-be.x; if(mCenter.SquareDistance(p)>=mRadius2) return FALSE;
p.x=bc.x+be.x; p.y=bc.y-be.y; if(mCenter.SquareDistance(p)>=mRadius2) return FALSE;
p.x=bc.x-be.x; if(mCenter.SquareDistance(p)>=mRadius2) return FALSE;
return TRUE;
}
#define TEST_BOX_IN_SPHERE(center, extents) \
if(SphereContainsBox(center, extents)) \
{ \
/* Set contact status */ \
mFlags |= OPC_CONTACT; \
_Dump(node); \
return; \
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for normal AABB trees.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void SphereCollider::_Collide(const AABBCollisionNode* node)
{
// Perform Sphere-AABB overlap test
if(!SphereAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents)) return;
TEST_BOX_IN_SPHERE(node->mAABB.mCenter, node->mAABB.mExtents)
if(node->IsLeaf())
{
SPHERE_PRIM(node->GetPrimitive(), OPC_CONTACT)
}
else
{
_Collide(node->GetPos());
if(ContactFound()) return;
_Collide(node->GetNeg());
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for normal AABB trees, without primitive tests.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void SphereCollider::_CollideNoPrimitiveTest(const AABBCollisionNode* node)
{
// Perform Sphere-AABB overlap test
if(!SphereAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents)) return;
TEST_BOX_IN_SPHERE(node->mAABB.mCenter, node->mAABB.mExtents)
if(node->IsLeaf())
{
SET_CONTACT(node->GetPrimitive(), OPC_CONTACT)
}
else
{
_CollideNoPrimitiveTest(node->GetPos());
if(ContactFound()) return;
_CollideNoPrimitiveTest(node->GetNeg());
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for quantized AABB trees.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void SphereCollider::_Collide(const AABBQuantizedNode* node)
{
// Dequantize box
const QuantizedAABB& Box = node->mAABB;
const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
// Perform Sphere-AABB overlap test
if(!SphereAABBOverlap(Center, Extents)) return;
TEST_BOX_IN_SPHERE(Center, Extents)
if(node->IsLeaf())
{
SPHERE_PRIM(node->GetPrimitive(), OPC_CONTACT)
}
else
{
_Collide(node->GetPos());
if(ContactFound()) return;
_Collide(node->GetNeg());
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for quantized AABB trees, without primitive tests.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void SphereCollider::_CollideNoPrimitiveTest(const AABBQuantizedNode* node)
{
// Dequantize box
const QuantizedAABB& Box = node->mAABB;
const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
// Perform Sphere-AABB overlap test
if(!SphereAABBOverlap(Center, Extents)) return;
TEST_BOX_IN_SPHERE(Center, Extents)
if(node->IsLeaf())
{
SET_CONTACT(node->GetPrimitive(), OPC_CONTACT)
}
else
{
_CollideNoPrimitiveTest(node->GetPos());
if(ContactFound()) return;
_CollideNoPrimitiveTest(node->GetNeg());
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for no-leaf AABB trees.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void SphereCollider::_Collide(const AABBNoLeafNode* node)
{
// Perform Sphere-AABB overlap test
if(!SphereAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents)) return;
TEST_BOX_IN_SPHERE(node->mAABB.mCenter, node->mAABB.mExtents)
if(node->HasPosLeaf()) { SPHERE_PRIM(node->GetPosPrimitive(), OPC_CONTACT) }
else _Collide(node->GetPos());
if(ContactFound()) return;
if(node->HasNegLeaf()) { SPHERE_PRIM(node->GetNegPrimitive(), OPC_CONTACT) }
else _Collide(node->GetNeg());
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for no-leaf AABB trees, without primitive tests.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void SphereCollider::_CollideNoPrimitiveTest(const AABBNoLeafNode* node)
{
// Perform Sphere-AABB overlap test
if(!SphereAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents)) return;
TEST_BOX_IN_SPHERE(node->mAABB.mCenter, node->mAABB.mExtents)
if(node->HasPosLeaf()) { SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) }
else _CollideNoPrimitiveTest(node->GetPos());
if(ContactFound()) return;
if(node->HasNegLeaf()) { SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) }
else _CollideNoPrimitiveTest(node->GetNeg());
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for quantized no-leaf AABB trees.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void SphereCollider::_Collide(const AABBQuantizedNoLeafNode* node)
{
// Dequantize box
const QuantizedAABB& Box = node->mAABB;
const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
// Perform Sphere-AABB overlap test
if(!SphereAABBOverlap(Center, Extents)) return;
TEST_BOX_IN_SPHERE(Center, Extents)
if(node->HasPosLeaf()) { SPHERE_PRIM(node->GetPosPrimitive(), OPC_CONTACT) }
else _Collide(node->GetPos());
if(ContactFound()) return;
if(node->HasNegLeaf()) { SPHERE_PRIM(node->GetNegPrimitive(), OPC_CONTACT) }
else _Collide(node->GetNeg());
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for quantized no-leaf AABB trees, without primitive tests.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void SphereCollider::_CollideNoPrimitiveTest(const AABBQuantizedNoLeafNode* node)
{
// Dequantize box
const QuantizedAABB& Box = node->mAABB;
const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
// Perform Sphere-AABB overlap test
if(!SphereAABBOverlap(Center, Extents)) return;
TEST_BOX_IN_SPHERE(Center, Extents)
if(node->HasPosLeaf()) { SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) }
else _CollideNoPrimitiveTest(node->GetPos());
if(ContactFound()) return;
if(node->HasNegLeaf()) { SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) }
else _CollideNoPrimitiveTest(node->GetNeg());
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for vanilla AABB trees.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void SphereCollider::_Collide(const AABBTreeNode* node)
{
// Perform Sphere-AABB overlap test
Point Center, Extents;
node->GetAABB()->GetCenter(Center);
node->GetAABB()->GetExtents(Extents);
if(!SphereAABBOverlap(Center, Extents)) return;
if(node->IsLeaf() || SphereContainsBox(Center, Extents))
{
mFlags |= OPC_CONTACT;
mTouchedPrimitives->Add(node->GetPrimitives(), node->GetNbPrimitives());
}
else
{
_Collide(node->GetPos());
_Collide(node->GetNeg());
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Constructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
HybridSphereCollider::HybridSphereCollider()
{
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Destructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
HybridSphereCollider::~HybridSphereCollider()
{
}
bool HybridSphereCollider::Collide(SphereCache& cache, const Sphere& sphere, const HybridModel& model, const Matrix4x4* worlds, const Matrix4x4* worldm)
{
// We don't want primitive tests here!
mFlags |= OPC_NO_PRIMITIVE_TESTS;
// Checkings
if(!Setup(&model)) return false;
// Init collision query
if(InitQuery(cache, sphere, worlds, worldm)) return true;
// Special case for 1-leaf trees
if(mCurrentModel && mCurrentModel->HasSingleNode())
{
// Here we're supposed to perform a normal query, except our tree has a single node, i.e. just a few triangles
udword Nb = mIMesh->GetNbTriangles();
// Loop through all triangles
for(udword i=0;i<Nb;i++)
{
SPHERE_PRIM(i, OPC_CONTACT)
}
return true;
}
// Override destination array since we're only going to get leaf boxes here
mTouchedBoxes.Reset();
mTouchedPrimitives = &mTouchedBoxes;
// Now, do the actual query against leaf boxes
if(!model.HasLeafNodes())
{
if(model.IsQuantized())
{
const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree();
// Setup dequantization coeffs
mCenterCoeff = Tree->mCenterCoeff;
mExtentsCoeff = Tree->mExtentsCoeff;
// Perform collision query - we don't want primitive tests here!
_CollideNoPrimitiveTest(Tree->GetNodes());
}
else
{
const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree();
// Perform collision query - we don't want primitive tests here!
_CollideNoPrimitiveTest(Tree->GetNodes());
}
}
else
{
if(model.IsQuantized())
{
const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree();
// Setup dequantization coeffs
mCenterCoeff = Tree->mCenterCoeff;
mExtentsCoeff = Tree->mExtentsCoeff;
// Perform collision query - we don't want primitive tests here!
_CollideNoPrimitiveTest(Tree->GetNodes());
}
else
{
const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree();
// Perform collision query - we don't want primitive tests here!
_CollideNoPrimitiveTest(Tree->GetNodes());
}
}
// We only have a list of boxes so far
if(GetContactStatus())
{
// Reset contact status, since it currently only reflects collisions with leaf boxes
Collider::InitQuery();
// Change dest container so that we can use built-in overlap tests and get collided primitives
cache.TouchedPrimitives.Reset();
mTouchedPrimitives = &cache.TouchedPrimitives;
// Read touched leaf boxes
udword Nb = mTouchedBoxes.GetNbEntries();
const udword* Touched = mTouchedBoxes.GetEntries();
const LeafTriangles* LT = model.GetLeafTriangles();
const udword* Indices = model.GetIndices();
// Loop through touched leaves
while(Nb--)
{
const LeafTriangles& CurrentLeaf = LT[*Touched++];
// Each leaf box has a set of triangles
udword NbTris = CurrentLeaf.GetNbTriangles();
if(Indices)
{
const udword* T = &Indices[CurrentLeaf.GetTriangleIndex()];
// Loop through triangles and test each of them
while(NbTris--)
{
udword TriangleIndex = *T++;
SPHERE_PRIM(TriangleIndex, OPC_CONTACT)
}
}
else
{
udword BaseIndex = CurrentLeaf.GetTriangleIndex();
// Loop through triangles and test each of them
while(NbTris--)
{
udword TriangleIndex = BaseIndex++;
SPHERE_PRIM(TriangleIndex, OPC_CONTACT)
}
}
}
}
return true;
}

96
ode/OPCODE/OPC_SphereCollider.h Normal file
View File

@ -0,0 +1,96 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for a sphere collider.
* \file OPC_SphereCollider.h
* \author Pierre Terdiman
* \date June, 2, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __OPC_SPHERECOLLIDER_H__
#define __OPC_SPHERECOLLIDER_H__
struct OPCODE_API SphereCache : VolumeCache
{
SphereCache() : Center(0.0f,0.0f,0.0f), FatRadius2(0.0f), FatCoeff(1.1f) {}
~SphereCache() {}
// Cached faces signature
Point Center; //!< Sphere used when performing the query resulting in cached faces
float FatRadius2; //!< Sphere used when performing the query resulting in cached faces
// User settings
float FatCoeff; //!< mRadius2 multiplier used to create a fat sphere
};
class OPCODE_API SphereCollider : public VolumeCollider
{
public:
// Constructor / Destructor
SphereCollider();
virtual ~SphereCollider();
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Generic collision query for generic OPCODE models. After the call, access the results:
* - with GetContactStatus()
* - with GetNbTouchedPrimitives()
* - with GetTouchedPrimitives()
*
* \param cache [in/out] a sphere cache
* \param sphere [in] collision sphere in local space
* \param model [in] Opcode model to collide with
* \param worlds [in] sphere's world matrix, or null
* \param worldm [in] model's world matrix, or null
* \return true if success
* \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool Collide(SphereCache& cache, const Sphere& sphere, const Model& model, const Matrix4x4* worlds=null, const Matrix4x4* worldm=null);
//
bool Collide(SphereCache& cache, const Sphere& sphere, const AABBTree* tree);
protected:
// Sphere in model space
Point mCenter; //!< Sphere center
float mRadius2; //!< Sphere radius squared
// Internal methods
void _Collide(const AABBCollisionNode* node);
void _Collide(const AABBNoLeafNode* node);
void _Collide(const AABBQuantizedNode* node);
void _Collide(const AABBQuantizedNoLeafNode* node);
void _Collide(const AABBTreeNode* node);
void _CollideNoPrimitiveTest(const AABBCollisionNode* node);
void _CollideNoPrimitiveTest(const AABBNoLeafNode* node);
void _CollideNoPrimitiveTest(const AABBQuantizedNode* node);
void _CollideNoPrimitiveTest(const AABBQuantizedNoLeafNode* node);
// Overlap tests
inline_ BOOL SphereContainsBox(const Point& bc, const Point& be);
inline_ BOOL SphereAABBOverlap(const Point& center, const Point& extents);
BOOL SphereTriOverlap(const Point& vert0, const Point& vert1, const Point& vert2);
// Init methods
BOOL InitQuery(SphereCache& cache, const Sphere& sphere, const Matrix4x4* worlds=null, const Matrix4x4* worldm=null);
};
class OPCODE_API HybridSphereCollider : public SphereCollider
{
public:
// Constructor / Destructor
HybridSphereCollider();
virtual ~HybridSphereCollider();
bool Collide(SphereCache& cache, const Sphere& sphere, const HybridModel& model, const Matrix4x4* worlds=null, const Matrix4x4* worldm=null);
protected:
Container mTouchedBoxes;
};
#endif // __OPC_SPHERECOLLIDER_H__

187
ode/OPCODE/OPC_SphereTriOverlap.h Normal file
View File

@ -0,0 +1,187 @@
// This is collision detection. If you do another distance test for collision *response*,
// if might be useful to simply *skip* the test below completely, and report a collision.
// - if sphere-triangle overlap, result is ok
// - if they don't, we'll discard them during collision response with a similar test anyway
// Overall this approach should run faster.
// Original code by David Eberly in Magic.
BOOL SphereCollider::SphereTriOverlap(const Point& vert0, const Point& vert1, const Point& vert2)
{
// Stats
mNbVolumePrimTests++;
// Early exit if one of the vertices is inside the sphere
Point kDiff = vert2 - mCenter;
float fC = kDiff.SquareMagnitude();
if(fC <= mRadius2) return TRUE;
kDiff = vert1 - mCenter;
fC = kDiff.SquareMagnitude();
if(fC <= mRadius2) return TRUE;
kDiff = vert0 - mCenter;
fC = kDiff.SquareMagnitude();
if(fC <= mRadius2) return TRUE;
// Else do the full distance test
Point TriEdge0 = vert1 - vert0;
Point TriEdge1 = vert2 - vert0;
//Point kDiff = vert0 - mCenter;
float fA00 = TriEdge0.SquareMagnitude();
float fA01 = TriEdge0 | TriEdge1;
float fA11 = TriEdge1.SquareMagnitude();
float fB0 = kDiff | TriEdge0;
float fB1 = kDiff | TriEdge1;
//float fC = kDiff.SquareMagnitude();
float fDet = fabsf(fA00*fA11 - fA01*fA01);
float u = fA01*fB1-fA11*fB0;
float v = fA01*fB0-fA00*fB1;
float SqrDist;
if(u + v <= fDet)
{
if(u < 0.0f)
{
if(v < 0.0f) // region 4
{
if(fB0 < 0.0f)
{
// v = 0.0f;
if(-fB0>=fA00) { /*u = 1.0f;*/ SqrDist = fA00+2.0f*fB0+fC; }
else { u = -fB0/fA00; SqrDist = fB0*u+fC; }
}
else
{
// u = 0.0f;
if(fB1>=0.0f) { /*v = 0.0f;*/ SqrDist = fC; }
else if(-fB1>=fA11) { /*v = 1.0f;*/ SqrDist = fA11+2.0f*fB1+fC; }
else { v = -fB1/fA11; SqrDist = fB1*v+fC; }
}
}
else // region 3
{
// u = 0.0f;
if(fB1>=0.0f) { /*v = 0.0f;*/ SqrDist = fC; }
else if(-fB1>=fA11) { /*v = 1.0f;*/ SqrDist = fA11+2.0f*fB1+fC; }
else { v = -fB1/fA11; SqrDist = fB1*v+fC; }
}
}
else if(v < 0.0f) // region 5
{
// v = 0.0f;
if(fB0>=0.0f) { /*u = 0.0f;*/ SqrDist = fC; }
else if(-fB0>=fA00) { /*u = 1.0f;*/ SqrDist = fA00+2.0f*fB0+fC; }
else { u = -fB0/fA00; SqrDist = fB0*u+fC; }
}
else // region 0
{
// minimum at interior point
if(fDet==0.0f)
{
// u = 0.0f;
// v = 0.0f;
SqrDist = MAX_FLOAT;
}
else
{
float fInvDet = 1.0f/fDet;
u *= fInvDet;
v *= fInvDet;
SqrDist = u*(fA00*u+fA01*v+2.0f*fB0) + v*(fA01*u+fA11*v+2.0f*fB1)+fC;
}
}
}
else
{
float fTmp0, fTmp1, fNumer, fDenom;
if(u < 0.0f) // region 2
{
fTmp0 = fA01 + fB0;
fTmp1 = fA11 + fB1;
if(fTmp1 > fTmp0)
{
fNumer = fTmp1 - fTmp0;
fDenom = fA00-2.0f*fA01+fA11;
if(fNumer >= fDenom)
{
// u = 1.0f;
// v = 0.0f;
SqrDist = fA00+2.0f*fB0+fC;
}
else
{
u = fNumer/fDenom;
v = 1.0f - u;
SqrDist = u*(fA00*u+fA01*v+2.0f*fB0) + v*(fA01*u+fA11*v+2.0f*fB1)+fC;
}
}
else
{
// u = 0.0f;
if(fTmp1 <= 0.0f) { /*v = 1.0f;*/ SqrDist = fA11+2.0f*fB1+fC; }
else if(fB1 >= 0.0f) { /*v = 0.0f;*/ SqrDist = fC; }
else { v = -fB1/fA11; SqrDist = fB1*v+fC; }
}
}
else if(v < 0.0f) // region 6
{
fTmp0 = fA01 + fB1;
fTmp1 = fA00 + fB0;
if(fTmp1 > fTmp0)
{
fNumer = fTmp1 - fTmp0;
fDenom = fA00-2.0f*fA01+fA11;
if(fNumer >= fDenom)
{
// v = 1.0f;
// u = 0.0f;
SqrDist = fA11+2.0f*fB1+fC;
}
else
{
v = fNumer/fDenom;
u = 1.0f - v;
SqrDist = u*(fA00*u+fA01*v+2.0f*fB0) + v*(fA01*u+fA11*v+2.0f*fB1)+fC;
}
}
else
{
// v = 0.0f;
if(fTmp1 <= 0.0f) { /*u = 1.0f;*/ SqrDist = fA00+2.0f*fB0+fC; }
else if(fB0 >= 0.0f) { /*u = 0.0f;*/ SqrDist = fC; }
else { u = -fB0/fA00; SqrDist = fB0*u+fC; }
}
}
else // region 1
{
fNumer = fA11 + fB1 - fA01 - fB0;
if(fNumer <= 0.0f)
{
// u = 0.0f;
// v = 1.0f;
SqrDist = fA11+2.0f*fB1+fC;
}
else
{
fDenom = fA00-2.0f*fA01+fA11;
if(fNumer >= fDenom)
{
// u = 1.0f;
// v = 0.0f;
SqrDist = fA00+2.0f*fB0+fC;
}
else
{
u = fNumer/fDenom;
v = 1.0f - u;
SqrDist = u*(fA00*u+fA01*v+2.0f*fB0) + v*(fA01*u+fA11*v+2.0f*fB1)+fC;
}
}
}
}
return fabsf(SqrDist) < mRadius2;
}

664
ode/OPCODE/OPC_SweepAndPrune.cpp Normal file
View File

@ -0,0 +1,664 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains an implementation of the sweep-and-prune algorithm (moved from Z-Collide)
* \file OPC_SweepAndPrune.cpp
* \author Pierre Terdiman
* \date January, 29, 2000
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Precompiled Header
#include "Stdafx.h"
using namespace Opcode;
inline_ void Sort(udword& id0, udword& id1)
{
if(id0>id1) Swap(id0, id1);
}
class Opcode::SAP_Element
{
public:
inline_ SAP_Element() {}
inline_ SAP_Element(udword id, SAP_Element* next) : mID(id), mNext(next) {}
inline_ ~SAP_Element() {}
udword mID;
SAP_Element* mNext;
};
class Opcode::SAP_Box
{
public:
SAP_EndPoint* Min[3];
SAP_EndPoint* Max[3];
};
class Opcode::SAP_EndPoint
{
public:
float Value; // Min or Max value
SAP_EndPoint* Previous; // Previous EndPoint whose Value is smaller than ours (or null)
SAP_EndPoint* Next; // Next EndPoint whose Value is greater than ours (or null)
udword Data; // Parent box ID *2 | MinMax flag
inline_ void SetData(udword box_id, BOOL is_max) { Data = (box_id<<1)|is_max; }
inline_ BOOL IsMax() const { return Data & 1; }
inline_ udword GetBoxID() const { return Data>>1; }
inline_ void InsertAfter(SAP_EndPoint* element)
{
if(this!=element && this!=element->Next)
{
// Remove
if(Previous) Previous->Next = Next;
if(Next) Next->Previous = Previous;
// Insert
Next = element->Next;
if(Next) Next->Previous = this;
element->Next = this;
Previous = element;
}
}
inline_ void InsertBefore(SAP_EndPoint* element)
{
if(this!=element && this!=element->Previous)
{
// Remove
if(Previous) Previous->Next = Next;
if(Next) Next->Previous = Previous;
// Insert
Previous = element->Previous;
element->Previous = this;
Next = element;
if(Previous) Previous->Next = this;
}
}
};
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Constructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
SAP_PairData::SAP_PairData() :
mNbElements (0),
mNbUsedElements (0),
mElementPool (null),
mFirstFree (null),
mNbObjects (0),
mArray (null)
{
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Destructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
SAP_PairData::~SAP_PairData()
{
Release();
}
void SAP_PairData::Release()
{
mNbElements = 0;
mNbUsedElements = 0;
mNbObjects = 0;
DELETEARRAY(mElementPool);
DELETEARRAY(mArray);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Initializes.
* \param nb_objects [in]
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool SAP_PairData::Init(udword nb_objects)
{
// Make sure everything has been released
Release();
if(!nb_objects) return false;
mArray = new SAP_Element*[nb_objects];
CHECKALLOC(mArray);
ZeroMemory(mArray, nb_objects*sizeof(SAP_Element*));
mNbObjects = nb_objects;
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Remaps a pointer when pool gets resized.
* \param element [in/out] remapped element
* \param delta [in] offset in bytes
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ void Remap(SAP_Element*& element, size_t delta)
{
if(element) element = (SAP_Element*)(size_t(element) + delta);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Gets a free element in the pool.
* \param id [in] element id
* \param next [in] next element
* \param remap [out] possible remapping offset
* \return the new element
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
SAP_Element* SAP_PairData::GetFreeElem(udword id, SAP_Element* next, udword* remap)
{
if(remap) *remap = 0;
SAP_Element* FreeElem;
if(mFirstFree)
{
// Recycle
FreeElem = mFirstFree;
mFirstFree = mFirstFree->mNext; // First free = next free (or null)
}
else
{
if(mNbUsedElements==mNbElements)
{
// Resize
mNbElements = mNbElements ? (mNbElements<<1) : 2;
SAP_Element* NewElems = new SAP_Element[mNbElements];
if(mNbUsedElements) CopyMemory(NewElems, mElementPool, mNbUsedElements*sizeof(SAP_Element));
// Remap everything
{
size_t Delta = size_t(NewElems) - size_t(mElementPool);
for(udword i=0;i<mNbUsedElements;i++) Remap(NewElems[i].mNext, Delta);
for(udword i=0;i<mNbObjects;i++) Remap(mArray[i], Delta);
Remap(mFirstFree, Delta);
Remap(next, Delta);
if(remap) *remap = Delta;
}
DELETEARRAY(mElementPool);
mElementPool = NewElems;
}
FreeElem = &mElementPool[mNbUsedElements++];
}
FreeElem->mID = id;
FreeElem->mNext = next;
return FreeElem;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Frees an element of the pool.
* \param elem [in] element to free/recycle
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ void SAP_PairData::FreeElem(SAP_Element* elem)
{
elem->mNext = mFirstFree; // Next free
mFirstFree = elem;
}
// Add a pair to the set.
void SAP_PairData::AddPair(udword id1, udword id2)
{
// Order the ids
Sort(id1, id2);
ASSERT(id1<mNbObjects);
if(id1>=mNbObjects) return;
// Select the right list from "mArray".
SAP_Element* Current = mArray[id1];
if(!Current)
{
// Empty slot => create new element
mArray[id1] = GetFreeElem(id2, null);
}
else if(Current->mID>id2)
{
// The list is not empty but all elements are greater than id2 => insert id2 in the front.
mArray[id1] = GetFreeElem(id2, mArray[id1]);
}
else
{
// Else find the correct location in the sorted list (ascending order) and insert id2 there.
while(Current->mNext)
{
if(Current->mNext->mID > id2) break;
Current = Current->mNext;
}
if(Current->mID==id2) return; // The pair already exists
// Current->mNext = GetFreeElem(id2, Current->mNext);
udword Delta;
SAP_Element* E = GetFreeElem(id2, Current->mNext, &Delta);
if(Delta) Remap(Current, Delta);
Current->mNext = E;
}
}
// Delete a pair from the set.
void SAP_PairData::RemovePair(udword id1, udword id2)
{
// Order the ids.
Sort(id1, id2);
// Exit if the pair doesn't exist in the set
if(id1>=mNbObjects) return;
// Otherwise, select the correct list.
SAP_Element* Current = mArray[id1];
// If this list is empty, the pair doesn't exist.
if(!Current) return;
// Otherwise, if id2 is the first element, delete it.
if(Current->mID==id2)
{
mArray[id1] = Current->mNext;
FreeElem(Current);
}
else
{
// If id2 is not the first element, start traversing the sorted list.
while(Current->mNext)
{
// If we have moved too far away without hitting id2, then the pair doesn't exist
if(Current->mNext->mID > id2) return;
// Otherwise, delete id2.
if(Current->mNext->mID == id2)
{
SAP_Element* Temp = Current->mNext;
Current->mNext = Temp->mNext;
FreeElem(Temp);
return;
}
Current = Current->mNext;
}
}
}
void SAP_PairData::DumpPairs(Pairs& pairs) const
{
// ### Ugly and slow
for(udword i=0;i<mNbObjects;i++)
{
SAP_Element* Current = mArray[i];
while(Current)
{
ASSERT(Current->mID<mNbObjects);
pairs.AddPair(i, Current->mID);
Current = Current->mNext;
}
}
}
void SAP_PairData::DumpPairs(PairCallback callback, void* user_data) const
{
if(!callback) return;
// ### Ugly and slow
for(udword i=0;i<mNbObjects;i++)
{
SAP_Element* Current = mArray[i];
while(Current)
{
ASSERT(Current->mID<mNbObjects);
if(!(callback)(i, Current->mID, user_data)) return;
Current = Current->mNext;
}
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Constructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
SweepAndPrune::SweepAndPrune()
{
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Destructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
SweepAndPrune::~SweepAndPrune()
{
}
void SweepAndPrune::GetPairs(Pairs& pairs) const
{
mPairs.DumpPairs(pairs);
}
void SweepAndPrune::GetPairs(PairCallback callback, void* user_data) const
{
mPairs.DumpPairs(callback, user_data);
}
bool SweepAndPrune::Init(udword nb_objects, const AABB** boxes)
{
// 1) Create sorted lists
mNbObjects = nb_objects;
mBoxes = new SAP_Box[nb_objects];
// for(udword i=0;i<nb_objects;i++) mBoxes[i].Box = *boxes[i];
float* Data = new float[nb_objects*2];
for(udword Axis=0;Axis<3;Axis++)
{
mList[Axis] = new SAP_EndPoint[nb_objects*2];
for(udword i=0;i<nb_objects;i++)
{
Data[i*2+0] = boxes[i]->GetMin(Axis);
Data[i*2+1] = boxes[i]->GetMax(Axis);
}
RadixSort RS;
const udword* Sorted = RS.Sort(Data, nb_objects*2).GetRanks();
SAP_EndPoint* PreviousEndPoint = null;
for(udword i=0;i<nb_objects*2;i++)
{
udword SortedIndex = *Sorted++;
float SortedCoord = Data[SortedIndex];
udword BoxIndex = SortedIndex>>1;
ASSERT(BoxIndex<nb_objects);
SAP_EndPoint* CurrentEndPoint = &mList[Axis][SortedIndex];
CurrentEndPoint->Value = SortedCoord;
// CurrentEndPoint->IsMax = SortedIndex&1; // ### could be implicit ?
// CurrentEndPoint->ID = BoxIndex; // ### could be implicit ?
CurrentEndPoint->SetData(BoxIndex, SortedIndex&1); // ### could be implicit ?
CurrentEndPoint->Previous = PreviousEndPoint;
CurrentEndPoint->Next = null;
if(PreviousEndPoint) PreviousEndPoint->Next = CurrentEndPoint;
if(CurrentEndPoint->IsMax()) mBoxes[BoxIndex].Max[Axis] = CurrentEndPoint;
else mBoxes[BoxIndex].Min[Axis] = CurrentEndPoint;
PreviousEndPoint = CurrentEndPoint;
}
}
DELETEARRAY(Data);
CheckListsIntegrity();
// 2) Quickly find starting pairs
mPairs.Init(nb_objects);
{
Pairs P;
CompleteBoxPruning(nb_objects, boxes, P, Axes(AXES_XZY));
for(udword i=0;i<P.GetNbPairs();i++)
{
const Pair* PP = P.GetPair(i);
udword id0 = PP->id0;
udword id1 = PP->id1;
if(id0!=id1 && boxes[id0]->Intersect(*boxes[id1]))
{
mPairs.AddPair(id0, id1);
}
else ASSERT(0);
}
}
return true;
}
bool SweepAndPrune::CheckListsIntegrity()
{
for(udword Axis=0;Axis<3;Axis++)
{
// Find list head
SAP_EndPoint* Current = mList[Axis];
while(Current->Previous) Current = Current->Previous;
udword Nb = 0;
SAP_EndPoint* Previous = null;
while(Current)
{
Nb++;
if(Previous)
{
ASSERT(Previous->Value <= Current->Value);
if(Previous->Value > Current->Value) return false;
}
ASSERT(Current->Previous==Previous);
if(Current->Previous!=Previous) return false;
Previous = Current;
Current = Current->Next;
}
ASSERT(Nb==mNbObjects*2);
}
return true;
}
inline_ BOOL Intersect(const AABB& a, const SAP_Box& b)
{
if(b.Max[0]->Value < a.GetMin(0) || a.GetMax(0) < b.Min[0]->Value
|| b.Max[1]->Value < a.GetMin(1) || a.GetMax(1) < b.Min[1]->Value
|| b.Max[2]->Value < a.GetMin(2) || a.GetMax(2) < b.Min[2]->Value) return FALSE;
return TRUE;
}
bool SweepAndPrune::UpdateObject(udword i, const AABB& box)
{
for(udword Axis=0;Axis<3;Axis++)
{
// udword Base = (udword)&mList[Axis][0];
// Update min
{
SAP_EndPoint* const CurrentMin = mBoxes[i].Min[Axis];
ASSERT(!CurrentMin->IsMax());
const float Limit = box.GetMin(Axis);
if(Limit == CurrentMin->Value)
{
}
else if(Limit < CurrentMin->Value)
{
CurrentMin->Value = Limit;
// Min is moving left:
SAP_EndPoint* NewPos = CurrentMin;
ASSERT(NewPos);
SAP_EndPoint* tmp;
while((tmp = NewPos->Previous) && tmp->Value > Limit)
{
NewPos = tmp;
if(NewPos->IsMax())
{
// Our min passed a max => start overlap
//udword SortedIndex = (udword(CurrentMin) - Base)/sizeof(NS_EndPoint);
const udword id0 = CurrentMin->GetBoxID();
const udword id1 = NewPos->GetBoxID();
if(id0!=id1 && Intersect(box, mBoxes[id1])) mPairs.AddPair(id0, id1);
}
}
CurrentMin->InsertBefore(NewPos);
}
else// if(Limit > CurrentMin->Value)
{
CurrentMin->Value = Limit;
// Min is moving right:
SAP_EndPoint* NewPos = CurrentMin;
ASSERT(NewPos);
SAP_EndPoint* tmp;
while((tmp = NewPos->Next) && tmp->Value < Limit)
{
NewPos = tmp;
if(NewPos->IsMax())
{
// Our min passed a max => stop overlap
const udword id0 = CurrentMin->GetBoxID();
const udword id1 = NewPos->GetBoxID();
if(id0!=id1) mPairs.RemovePair(id0, id1);
}
}
CurrentMin->InsertAfter(NewPos);
}
}
// Update max
{
SAP_EndPoint* const CurrentMax = mBoxes[i].Max[Axis];
ASSERT(CurrentMax->IsMax());
const float Limit = box.GetMax(Axis);
if(Limit == CurrentMax->Value)
{
}
else if(Limit > CurrentMax->Value)
{
CurrentMax->Value = Limit;
// Max is moving right:
SAP_EndPoint* NewPos = CurrentMax;
ASSERT(NewPos);
SAP_EndPoint* tmp;
while((tmp = NewPos->Next) && tmp->Value < Limit)
{
NewPos = tmp;
if(!NewPos->IsMax())
{
// Our max passed a min => start overlap
const udword id0 = CurrentMax->GetBoxID();
const udword id1 = NewPos->GetBoxID();
if(id0!=id1 && Intersect(box, mBoxes[id1])) mPairs.AddPair(id0, id1);
}
}
CurrentMax->InsertAfter(NewPos);
}
else// if(Limit < CurrentMax->Value)
{
CurrentMax->Value = Limit;
// Max is moving left:
SAP_EndPoint* NewPos = CurrentMax;
ASSERT(NewPos);
SAP_EndPoint* tmp;
while((tmp = NewPos->Previous) && tmp->Value > Limit)
{
NewPos = tmp;
if(!NewPos->IsMax())
{
// Our max passed a min => stop overlap
const udword id0 = CurrentMax->GetBoxID();
const udword id1 = NewPos->GetBoxID();
if(id0!=id1) mPairs.RemovePair(id0, id1);
}
}
CurrentMax->InsertBefore(NewPos);
}
}
}
return true;
}

86
ode/OPCODE/OPC_SweepAndPrune.h Normal file
View File

@ -0,0 +1,86 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains an implementation of the sweep-and-prune algorithm (moved from Z-Collide)
* \file OPC_SweepAndPrune.h
* \author Pierre Terdiman
* \date January, 29, 2000
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __OPC_SWEEPANDPRUNE_H__
#define __OPC_SWEEPANDPRUNE_H__
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* User-callback, called by OPCODE for each colliding pairs.
* \param id0 [in] id of colliding object
* \param id1 [in] id of colliding object
* \param user_data [in] user-defined data
* \return TRUE to continue enumeration
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
typedef BOOL (*PairCallback) (udword id0, udword id1, void* user_data);
class SAP_Element;
class SAP_EndPoint;
class SAP_Box;
class OPCODE_API SAP_PairData
{
public:
SAP_PairData();
~SAP_PairData();
bool Init(udword nb_objects);
void AddPair(udword id1, udword id2);
void RemovePair(udword id1, udword id2);
void DumpPairs(Pairs& pairs) const;
void DumpPairs(PairCallback callback, void* user_data) const;
private:
udword mNbElements; //!< Total number of elements in the pool
udword mNbUsedElements; //!< Number of used elements
SAP_Element* mElementPool; //!< Array of mNbElements elements
SAP_Element* mFirstFree; //!< First free element in the pool
udword mNbObjects; //!< Max number of objects we can handle
SAP_Element** mArray; //!< Pointers to pool
// Internal methods
SAP_Element* GetFreeElem(udword id, SAP_Element* next, udword* remap=null);
inline_ void FreeElem(SAP_Element* elem);
void Release();
};
class OPCODE_API SweepAndPrune
{
public:
SweepAndPrune();
~SweepAndPrune();
bool Init(udword nb_objects, const AABB** boxes);
bool UpdateObject(udword i, const AABB& box);
void GetPairs(Pairs& pairs) const;
void GetPairs(PairCallback callback, void* user_data) const;
private:
SAP_PairData mPairs;
udword mNbObjects;
SAP_Box* mBoxes;
SAP_EndPoint* mList[3];
// Internal methods
bool CheckListsIntegrity();
};
#endif //__OPC_SWEEPANDPRUNE_H__

255
ode/OPCODE/OPC_TreeBuilders.cpp Normal file
View File

@ -0,0 +1,255 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for tree builders.
* \file OPC_TreeBuilders.cpp
* \author Pierre Terdiman
* \date March, 20, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* A builder for AABB-trees of vertices.
*
* \class AABBTreeOfVerticesBuilder
* \author Pierre Terdiman
* \version 1.3
* \date March, 20, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* A builder for AABB-trees of AABBs.
*
* \class AABBTreeOfAABBsBuilder
* \author Pierre Terdiman
* \version 1.3
* \date March, 20, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* A builder for AABB-trees of triangles.
*
* \class AABBTreeOfTrianglesBuilder
* \author Pierre Terdiman
* \version 1.3
* \date March, 20, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Precompiled Header
#include "Stdafx.h"
using namespace Opcode;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the AABB of a set of primitives.
* \param primitives [in] list of indices of primitives
* \param nb_prims [in] number of indices
* \param global_box [out] global AABB enclosing the set of input primitives
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool AABBTreeOfAABBsBuilder::ComputeGlobalBox(const udword* primitives, udword nb_prims, AABB& global_box) const
{
// Checkings
if(!primitives || !nb_prims) return false;
// Initialize global box
global_box = mAABBArray[primitives[0]];
// Loop through boxes
for(udword i=1;i<nb_prims;i++)
{
// Update global box
global_box.Add(mAABBArray[primitives[i]]);
}
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the splitting value along a given axis for a given primitive.
* \param index [in] index of the primitive to split
* \param axis [in] axis index (0,1,2)
* \return splitting value
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
float AABBTreeOfAABBsBuilder::GetSplittingValue(udword index, udword axis) const
{
// For an AABB, the splitting value is the middle of the given axis,
// i.e. the corresponding component of the center point
return mAABBArray[index].GetCenter(axis);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the AABB of a set of primitives.
* \param primitives [in] list of indices of primitives
* \param nb_prims [in] number of indices
* \param global_box [out] global AABB enclosing the set of input primitives
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool AABBTreeOfTrianglesBuilder::ComputeGlobalBox(const udword* primitives, udword nb_prims, AABB& global_box) const
{
// Checkings
if(!primitives || !nb_prims) return false;
// Initialize global box
Point Min(MAX_FLOAT, MAX_FLOAT, MAX_FLOAT);
Point Max(MIN_FLOAT, MIN_FLOAT, MIN_FLOAT);
// Loop through triangles
VertexPointers VP;
while(nb_prims--)
{
// Get current triangle-vertices
mIMesh->GetTriangle(VP, *primitives++);
// Update global box
Min.Min(*VP.Vertex[0]).Min(*VP.Vertex[1]).Min(*VP.Vertex[2]);
Max.Max(*VP.Vertex[0]).Max(*VP.Vertex[1]).Max(*VP.Vertex[2]);
}
global_box.SetMinMax(Min, Max);
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the splitting value along a given axis for a given primitive.
* \param index [in] index of the primitive to split
* \param axis [in] axis index (0,1,2)
* \return splitting value
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
float AABBTreeOfTrianglesBuilder::GetSplittingValue(udword index, udword axis) const
{
/* // Compute center of triangle
Point Center;
mTriList[index].Center(mVerts, Center);
// Return value
return Center[axis];*/
// Compute correct component from center of triangle
// return (mVerts[mTriList[index].mVRef[0]][axis]
// +mVerts[mTriList[index].mVRef[1]][axis]
// +mVerts[mTriList[index].mVRef[2]][axis])*INV3;
VertexPointers VP;
mIMesh->GetTriangle(VP, index);
// Compute correct component from center of triangle
return ((*VP.Vertex[0])[axis]
+(*VP.Vertex[1])[axis]
+(*VP.Vertex[2])[axis])*INV3;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the splitting value along a given axis for a given node.
* \param primitives [in] list of indices of primitives
* \param nb_prims [in] number of indices
* \param global_box [in] global AABB enclosing the set of input primitives
* \param axis [in] axis index (0,1,2)
* \return splitting value
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
float AABBTreeOfTrianglesBuilder::GetSplittingValue(const udword* primitives, udword nb_prims, const AABB& global_box, udword axis) const
{
if(mSettings.mRules&SPLIT_GEOM_CENTER)
{
// Loop through triangles
float SplitValue = 0.0f;
VertexPointers VP;
for(udword i=0;i<nb_prims;i++)
{
// Get current triangle-vertices
mIMesh->GetTriangle(VP, primitives[i]);
// Update split value
SplitValue += (*VP.Vertex[0])[axis];
SplitValue += (*VP.Vertex[1])[axis];
SplitValue += (*VP.Vertex[2])[axis];
}
return SplitValue / float(nb_prims*3);
}
else return AABBTreeBuilder::GetSplittingValue(primitives, nb_prims, global_box, axis);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the AABB of a set of primitives.
* \param primitives [in] list of indices of primitives
* \param nb_prims [in] number of indices
* \param global_box [out] global AABB enclosing the set of input primitives
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool AABBTreeOfVerticesBuilder::ComputeGlobalBox(const udword* primitives, udword nb_prims, AABB& global_box) const
{
// Checkings
if(!primitives || !nb_prims) return false;
// Initialize global box
global_box.SetEmpty();
// Loop through vertices
for(udword i=0;i<nb_prims;i++)
{
// Update global box
global_box.Extend(mVertexArray[primitives[i]]);
}
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the splitting value along a given axis for a given primitive.
* \param index [in] index of the primitive to split
* \param axis [in] axis index (0,1,2)
* \return splitting value
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
float AABBTreeOfVerticesBuilder::GetSplittingValue(udword index, udword axis) const
{
// For a vertex, the splitting value is simply the vertex coordinate.
return mVertexArray[index][axis];
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the splitting value along a given axis for a given node.
* \param primitives [in] list of indices of primitives
* \param nb_prims [in] number of indices
* \param global_box [in] global AABB enclosing the set of input primitives
* \param axis [in] axis index (0,1,2)
* \return splitting value
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
float AABBTreeOfVerticesBuilder::GetSplittingValue(const udword* primitives, udword nb_prims, const AABB& global_box, udword axis) const
{
if(mSettings.mRules&SPLIT_GEOM_CENTER)
{
// Loop through vertices
float SplitValue = 0.0f;
for(udword i=0;i<nb_prims;i++)
{
// Update split value
SplitValue += mVertexArray[primitives[i]][axis];
}
return SplitValue / float(nb_prims);
}
else return AABBTreeBuilder::GetSplittingValue(primitives, nb_prims, global_box, axis);
}

173
ode/OPCODE/OPC_TreeBuilders.h Normal file
View File

@ -0,0 +1,173 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for tree builders.
* \file OPC_TreeBuilders.h
* \author Pierre Terdiman
* \date March, 20, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __OPC_TREEBUILDERS_H__
#define __OPC_TREEBUILDERS_H__
//! Tree splitting rules
enum SplittingRules
{
// Primitive split
SPLIT_LARGEST_AXIS = (1<<0), //!< Split along the largest axis
SPLIT_SPLATTER_POINTS = (1<<1), //!< Splatter primitive centers (QuickCD-style)
SPLIT_BEST_AXIS = (1<<2), //!< Try largest axis, then second, then last
SPLIT_BALANCED = (1<<3), //!< Try to keep a well-balanced tree
SPLIT_FIFTY = (1<<4), //!< Arbitrary 50-50 split
// Node split
SPLIT_GEOM_CENTER = (1<<5), //!< Split at geometric center (else split in the middle)
//
SPLIT_FORCE_DWORD = 0x7fffffff
};
//! Simple wrapper around build-related settings [Opcode 1.3]
struct OPCODE_API BuildSettings
{
inline_ BuildSettings() : mLimit(1), mRules(SPLIT_FORCE_DWORD) {}
udword mLimit; //!< Limit number of primitives / node. If limit is 1, build a complete tree (2*N-1 nodes)
udword mRules; //!< Building/Splitting rules (a combination of SplittingRules flags)
};
class OPCODE_API AABBTreeBuilder
{
public:
//! Constructor
AABBTreeBuilder() :
mNbPrimitives(0),
mNodeBase(null),
mCount(0),
mNbInvalidSplits(0) {}
//! Destructor
virtual ~AABBTreeBuilder() {}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the AABB of a set of primitives.
* \param primitives [in] list of indices of primitives
* \param nb_prims [in] number of indices
* \param global_box [out] global AABB enclosing the set of input primitives
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
virtual bool ComputeGlobalBox(const udword* primitives, udword nb_prims, AABB& global_box) const = 0;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the splitting value along a given axis for a given primitive.
* \param index [in] index of the primitive to split
* \param axis [in] axis index (0,1,2)
* \return splitting value
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
virtual float GetSplittingValue(udword index, udword axis) const = 0;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Computes the splitting value along a given axis for a given node.
* \param primitives [in] list of indices of primitives
* \param nb_prims [in] number of indices
* \param global_box [in] global AABB enclosing the set of input primitives
* \param axis [in] axis index (0,1,2)
* \return splitting value
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
virtual float GetSplittingValue(const udword* primitives, udword nb_prims, const AABB& global_box, udword axis) const
{
// Default split value = middle of the axis (using only the box)
return global_box.GetCenter(axis);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Validates node subdivision. This is called each time a node is considered for subdivision, during tree building.
* \param primitives [in] list of indices of primitives
* \param nb_prims [in] number of indices
* \param global_box [in] global AABB enclosing the set of input primitives
* \return TRUE if the node should be subdivised
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
virtual BOOL ValidateSubdivision(const udword* primitives, udword nb_prims, const AABB& global_box)
{
// Check the user-defined limit
if(nb_prims<=mSettings.mLimit) return FALSE;
return TRUE;
}
BuildSettings mSettings; //!< Splitting rules & split limit [Opcode 1.3]
udword mNbPrimitives; //!< Total number of primitives.
void* mNodeBase; //!< Address of node pool [Opcode 1.3]
// Stats
inline_ void SetCount(udword nb) { mCount=nb; }
inline_ void IncreaseCount(udword nb) { mCount+=nb; }
inline_ udword GetCount() const { return mCount; }
inline_ void SetNbInvalidSplits(udword nb) { mNbInvalidSplits=nb; }
inline_ void IncreaseNbInvalidSplits() { mNbInvalidSplits++; }
inline_ udword GetNbInvalidSplits() const { return mNbInvalidSplits; }
private:
udword mCount; //!< Stats: number of nodes created
udword mNbInvalidSplits; //!< Stats: number of invalid splits
};
class OPCODE_API AABBTreeOfVerticesBuilder : public AABBTreeBuilder
{
public:
//! Constructor
AABBTreeOfVerticesBuilder() : mVertexArray(null) {}
//! Destructor
virtual ~AABBTreeOfVerticesBuilder() {}
override(AABBTreeBuilder) bool ComputeGlobalBox(const udword* primitives, udword nb_prims, AABB& global_box) const;
override(AABBTreeBuilder) float GetSplittingValue(udword index, udword axis) const;
override(AABBTreeBuilder) float GetSplittingValue(const udword* primitives, udword nb_prims, const AABB& global_box, udword axis) const;
const Point* mVertexArray; //!< Shortcut to an app-controlled array of vertices.
};
class OPCODE_API AABBTreeOfAABBsBuilder : public AABBTreeBuilder
{
public:
//! Constructor
AABBTreeOfAABBsBuilder() : mAABBArray(null) {}
//! Destructor
virtual ~AABBTreeOfAABBsBuilder() {}
override(AABBTreeBuilder) bool ComputeGlobalBox(const udword* primitives, udword nb_prims, AABB& global_box) const;
override(AABBTreeBuilder) float GetSplittingValue(udword index, udword axis) const;
const AABB* mAABBArray; //!< Shortcut to an app-controlled array of AABBs.
};
class OPCODE_API AABBTreeOfTrianglesBuilder : public AABBTreeBuilder
{
public:
//! Constructor
AABBTreeOfTrianglesBuilder() : mIMesh(null) {}
//! Destructor
virtual ~AABBTreeOfTrianglesBuilder() {}
override(AABBTreeBuilder) bool ComputeGlobalBox(const udword* primitives, udword nb_prims, AABB& global_box) const;
override(AABBTreeBuilder) float GetSplittingValue(udword index, udword axis) const;
override(AABBTreeBuilder) float GetSplittingValue(const udword* primitives, udword nb_prims, const AABB& global_box, udword axis) const;
const MeshInterface* mIMesh; //!< Shortcut to an app-controlled mesh interface
};
#endif // __OPC_TREEBUILDERS_H__

943
ode/OPCODE/OPC_TreeCollider.cpp Normal file
View File

@ -0,0 +1,943 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for a tree collider.
* \file OPC_TreeCollider.cpp
* \author Pierre Terdiman
* \date March, 20, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains an AABB tree collider.
* This class performs a collision test between two AABB trees.
*
* \class AABBTreeCollider
* \author Pierre Terdiman
* \version 1.3
* \date March, 20, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Precompiled Header
#include "Stdafx.h"
using namespace Opcode;
#include "OPC_BoxBoxOverlap.h"
#include "OPC_TriBoxOverlap.h"
#include "OPC_TriTriOverlap.h"
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Constructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
AABBTreeCollider::AABBTreeCollider() :
mNbBVBVTests (0),
mNbPrimPrimTests (0),
mNbBVPrimTests (0),
mFullBoxBoxTest (true),
mFullPrimBoxTest (true),
mIMesh0 (null),
mIMesh1 (null)
{
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Destructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
AABBTreeCollider::~AABBTreeCollider()
{
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Validates current settings. You should call this method after all the settings and callbacks have been defined.
* \return null if everything is ok, else a string describing the problem
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
const char* AABBTreeCollider::ValidateSettings()
{
if(TemporalCoherenceEnabled() && !FirstContactEnabled()) return "Temporal coherence only works with ""First contact"" mode!";
return null;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Generic collision query for generic OPCODE models. After the call, access the results with:
* - GetContactStatus()
* - GetNbPairs()
* - GetPairs()
*
* \param cache [in] collision cache for model pointers and a colliding pair of primitives
* \param world0 [in] world matrix for first object
* \param world1 [in] world matrix for second object
* \return true if success
* \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool AABBTreeCollider::Collide(BVTCache& cache, const Matrix4x4* world0, const Matrix4x4* world1)
{
// Checkings
if(!cache.Model0 || !cache.Model1) return false;
if(cache.Model0->HasLeafNodes()!=cache.Model1->HasLeafNodes()) return false;
if(cache.Model0->IsQuantized()!=cache.Model1->IsQuantized()) return false;
/*
Rules:
- perform hull test
- when hulls collide, disable hull test
- if meshes overlap, reset countdown
- if countdown reaches 0, enable hull test
*/
#ifdef __MESHMERIZER_H__
// Handle hulls
if(cache.HullTest)
{
if(cache.Model0->GetHull() && cache.Model1->GetHull())
{
struct Local
{
static Point* SVCallback(const Point& sv, udword& previndex, udword user_data)
{
CollisionHull* Hull = (CollisionHull*)user_data;
previndex = Hull->ComputeSupportingVertex(sv, previndex);
return (Point*)&Hull->GetVerts()[previndex];
}
};
bool Collide;
if(0)
{
static GJKEngine GJK;
static bool GJKInitDone=false;
if(!GJKInitDone)
{
GJK.Enable(GJK_BACKUP_PROCEDURE);
GJK.Enable(GJK_DEGENERATE);
GJK.Enable(GJK_HILLCLIMBING);
GJKInitDone = true;
}
GJK.SetCallbackObj0(Local::SVCallback);
GJK.SetCallbackObj1(Local::SVCallback);
GJK.SetUserData0(udword(cache.Model0->GetHull()));
GJK.SetUserData1(udword(cache.Model1->GetHull()));
Collide = GJK.Collide(*world0, *world1, &cache.SepVector);
}
else
{
static SVEngine SVE;
SVE.SetCallbackObj0(Local::SVCallback);
SVE.SetCallbackObj1(Local::SVCallback);
SVE.SetUserData0(udword(cache.Model0->GetHull()));
SVE.SetUserData1(udword(cache.Model1->GetHull()));
Collide = SVE.Collide(*world0, *world1, &cache.SepVector);
}
if(!Collide)
{
// Reset stats & contact status
mFlags &= ~OPC_CONTACT;
mNbBVBVTests = 0;
mNbPrimPrimTests = 0;
mNbBVPrimTests = 0;
mPairs.Reset();
return true;
}
}
}
// Here, hulls collide
cache.HullTest = false;
#endif // __MESHMERIZER_H__
// Checkings
if(!Setup(cache.Model0->GetMeshInterface(), cache.Model1->GetMeshInterface())) return false;
// Simple double-dispatch
bool Status;
if(!cache.Model0->HasLeafNodes())
{
if(cache.Model0->IsQuantized())
{
const AABBQuantizedNoLeafTree* T0 = (const AABBQuantizedNoLeafTree*)cache.Model0->GetTree();
const AABBQuantizedNoLeafTree* T1 = (const AABBQuantizedNoLeafTree*)cache.Model1->GetTree();
Status = Collide(T0, T1, world0, world1, &cache);
}
else
{
const AABBNoLeafTree* T0 = (const AABBNoLeafTree*)cache.Model0->GetTree();
const AABBNoLeafTree* T1 = (const AABBNoLeafTree*)cache.Model1->GetTree();
Status = Collide(T0, T1, world0, world1, &cache);
}
}
else
{
if(cache.Model0->IsQuantized())
{
const AABBQuantizedTree* T0 = (const AABBQuantizedTree*)cache.Model0->GetTree();
const AABBQuantizedTree* T1 = (const AABBQuantizedTree*)cache.Model1->GetTree();
Status = Collide(T0, T1, world0, world1, &cache);
}
else
{
const AABBCollisionTree* T0 = (const AABBCollisionTree*)cache.Model0->GetTree();
const AABBCollisionTree* T1 = (const AABBCollisionTree*)cache.Model1->GetTree();
Status = Collide(T0, T1, world0, world1, &cache);
}
}
#ifdef __MESHMERIZER_H__
if(Status)
{
// Reset counter as long as overlap occurs
if(GetContactStatus()) cache.ResetCountDown();
// Enable hull test again when counter reaches zero
cache.CountDown--;
if(!cache.CountDown)
{
cache.ResetCountDown();
cache.HullTest = true;
}
}
#endif
return Status;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Initializes a collision query :
* - reset stats & contact status
* - setup matrices
*
* \param world0 [in] world matrix for first object
* \param world1 [in] world matrix for second object
* \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void AABBTreeCollider::InitQuery(const Matrix4x4* world0, const Matrix4x4* world1)
{
// Reset stats & contact status
Collider::InitQuery();
mNbBVBVTests = 0;
mNbPrimPrimTests = 0;
mNbBVPrimTests = 0;
mPairs.Reset();
// Setup matrices
Matrix4x4 InvWorld0, InvWorld1;
if(world0) InvertPRMatrix(InvWorld0, *world0);
else InvWorld0.Identity();
if(world1) InvertPRMatrix(InvWorld1, *world1);
else InvWorld1.Identity();
Matrix4x4 World0to1 = world0 ? (*world0 * InvWorld1) : InvWorld1;
Matrix4x4 World1to0 = world1 ? (*world1 * InvWorld0) : InvWorld0;
mR0to1 = World0to1; World0to1.GetTrans(mT0to1);
mR1to0 = World1to0; World1to0.GetTrans(mT1to0);
// Precompute absolute 1-to-0 rotation matrix
for(udword i=0;i<3;i++)
{
for(udword j=0;j<3;j++)
{
// Epsilon value prevents floating-point inaccuracies (strategy borrowed from RAPID)
mAR.m[i][j] = 1e-6f + fabsf(mR1to0.m[i][j]);
}
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Takes advantage of temporal coherence.
* \param cache [in] cache for a pair of previously colliding primitives
* \return true if we can return immediately
* \warning only works for "First Contact" mode
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool AABBTreeCollider::CheckTemporalCoherence(Pair* cache)
{
// Checkings
if(!cache) return false;
// Test previously colliding primitives first
if(TemporalCoherenceEnabled() && FirstContactEnabled())
{
PrimTest(cache->id0, cache->id1);
if(GetContactStatus()) return true;
}
return false;
}
#define UPDATE_CACHE \
if(cache && GetContactStatus()) \
{ \
cache->id0 = mPairs.GetEntry(0); \
cache->id1 = mPairs.GetEntry(1); \
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Collision query for normal AABB trees.
* \param tree0 [in] AABB tree from first object
* \param tree1 [in] AABB tree from second object
* \param world0 [in] world matrix for first object
* \param world1 [in] world matrix for second object
* \param cache [in/out] cache for a pair of previously colliding primitives
* \return true if success
* \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool AABBTreeCollider::Collide(const AABBCollisionTree* tree0, const AABBCollisionTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache)
{
// Init collision query
InitQuery(world0, world1);
// Check previous state
if(CheckTemporalCoherence(cache)) return true;
// Perform collision query
_Collide(tree0->GetNodes(), tree1->GetNodes());
UPDATE_CACHE
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Collision query for no-leaf AABB trees.
* \param tree0 [in] AABB tree from first object
* \param tree1 [in] AABB tree from second object
* \param world0 [in] world matrix for first object
* \param world1 [in] world matrix for second object
* \param cache [in/out] cache for a pair of previously colliding primitives
* \return true if success
* \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool AABBTreeCollider::Collide(const AABBNoLeafTree* tree0, const AABBNoLeafTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache)
{
// Init collision query
InitQuery(world0, world1);
// Check previous state
if(CheckTemporalCoherence(cache)) return true;
// Perform collision query
_Collide(tree0->GetNodes(), tree1->GetNodes());
UPDATE_CACHE
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Collision query for quantized AABB trees.
* \param tree0 [in] AABB tree from first object
* \param tree1 [in] AABB tree from second object
* \param world0 [in] world matrix for first object
* \param world1 [in] world matrix for second object
* \param cache [in/out] cache for a pair of previously colliding primitives
* \return true if success
* \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool AABBTreeCollider::Collide(const AABBQuantizedTree* tree0, const AABBQuantizedTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache)
{
// Init collision query
InitQuery(world0, world1);
// Check previous state
if(CheckTemporalCoherence(cache)) return true;
// Setup dequantization coeffs
mCenterCoeff0 = tree0->mCenterCoeff;
mExtentsCoeff0 = tree0->mExtentsCoeff;
mCenterCoeff1 = tree1->mCenterCoeff;
mExtentsCoeff1 = tree1->mExtentsCoeff;
// Dequantize box A
const AABBQuantizedNode* N0 = tree0->GetNodes();
const Point a(float(N0->mAABB.mExtents[0]) * mExtentsCoeff0.x, float(N0->mAABB.mExtents[1]) * mExtentsCoeff0.y, float(N0->mAABB.mExtents[2]) * mExtentsCoeff0.z);
const Point Pa(float(N0->mAABB.mCenter[0]) * mCenterCoeff0.x, float(N0->mAABB.mCenter[1]) * mCenterCoeff0.y, float(N0->mAABB.mCenter[2]) * mCenterCoeff0.z);
// Dequantize box B
const AABBQuantizedNode* N1 = tree1->GetNodes();
const Point b(float(N1->mAABB.mExtents[0]) * mExtentsCoeff1.x, float(N1->mAABB.mExtents[1]) * mExtentsCoeff1.y, float(N1->mAABB.mExtents[2]) * mExtentsCoeff1.z);
const Point Pb(float(N1->mAABB.mCenter[0]) * mCenterCoeff1.x, float(N1->mAABB.mCenter[1]) * mCenterCoeff1.y, float(N1->mAABB.mCenter[2]) * mCenterCoeff1.z);
// Perform collision query
_Collide(N0, N1, a, Pa, b, Pb);
UPDATE_CACHE
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Collision query for quantized no-leaf AABB trees.
* \param tree0 [in] AABB tree from first object
* \param tree1 [in] AABB tree from second object
* \param world0 [in] world matrix for first object
* \param world1 [in] world matrix for second object
* \param cache [in/out] cache for a pair of previously colliding primitives
* \return true if success
* \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool AABBTreeCollider::Collide(const AABBQuantizedNoLeafTree* tree0, const AABBQuantizedNoLeafTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache)
{
// Init collision query
InitQuery(world0, world1);
// Check previous state
if(CheckTemporalCoherence(cache)) return true;
// Setup dequantization coeffs
mCenterCoeff0 = tree0->mCenterCoeff;
mExtentsCoeff0 = tree0->mExtentsCoeff;
mCenterCoeff1 = tree1->mCenterCoeff;
mExtentsCoeff1 = tree1->mExtentsCoeff;
// Perform collision query
_Collide(tree0->GetNodes(), tree1->GetNodes());
UPDATE_CACHE
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Standard trees
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// The normal AABB tree can use 2 different descent rules (with different performances)
//#define ORIGINAL_CODE //!< UNC-like descent rules
#define ALTERNATIVE_CODE //!< Alternative descent rules
#ifdef ORIGINAL_CODE
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for normal AABB trees.
* \param b0 [in] collision node from first tree
* \param b1 [in] collision node from second tree
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void AABBTreeCollider::_Collide(const AABBCollisionNode* b0, const AABBCollisionNode* b1)
{
// Perform BV-BV overlap test
if(!BoxBoxOverlap(b0->mAABB.mExtents, b0->mAABB.mCenter, b1->mAABB.mExtents, b1->mAABB.mCenter)) return;
if(b0->IsLeaf() && b1->IsLeaf()) { PrimTest(b0->GetPrimitive(), b1->GetPrimitive()); return; }
if(b1->IsLeaf() || (!b0->IsLeaf() && (b0->GetSize() > b1->GetSize())))
{
_Collide(b0->GetNeg(), b1);
if(ContactFound()) return;
_Collide(b0->GetPos(), b1);
}
else
{
_Collide(b0, b1->GetNeg());
if(ContactFound()) return;
_Collide(b0, b1->GetPos());
}
}
#endif
#ifdef ALTERNATIVE_CODE
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for normal AABB trees.
* \param b0 [in] collision node from first tree
* \param b1 [in] collision node from second tree
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void AABBTreeCollider::_Collide(const AABBCollisionNode* b0, const AABBCollisionNode* b1)
{
// Perform BV-BV overlap test
if(!BoxBoxOverlap(b0->mAABB.mExtents, b0->mAABB.mCenter, b1->mAABB.mExtents, b1->mAABB.mCenter))
{
return;
}
if(b0->IsLeaf())
{
if(b1->IsLeaf())
{
PrimTest(b0->GetPrimitive(), b1->GetPrimitive());
}
else
{
_Collide(b0, b1->GetNeg());
if(ContactFound()) return;
_Collide(b0, b1->GetPos());
}
}
else if(b1->IsLeaf())
{
_Collide(b0->GetNeg(), b1);
if(ContactFound()) return;
_Collide(b0->GetPos(), b1);
}
else
{
_Collide(b0->GetNeg(), b1->GetNeg());
if(ContactFound()) return;
_Collide(b0->GetNeg(), b1->GetPos());
if(ContactFound()) return;
_Collide(b0->GetPos(), b1->GetNeg());
if(ContactFound()) return;
_Collide(b0->GetPos(), b1->GetPos());
}
}
#endif
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// No-leaf trees
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Leaf-leaf test for two primitive indices.
* \param id0 [in] index from first leaf-triangle
* \param id1 [in] index from second leaf-triangle
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void AABBTreeCollider::PrimTest(udword id0, udword id1)
{
// Request vertices from the app
VertexPointers VP0;
VertexPointers VP1;
mIMesh0->GetTriangle(VP0, id0);
mIMesh1->GetTriangle(VP1, id1);
// Transform from space 1 to space 0
Point u0,u1,u2;
TransformPoint(u0, *VP1.Vertex[0], mR1to0, mT1to0);
TransformPoint(u1, *VP1.Vertex[1], mR1to0, mT1to0);
TransformPoint(u2, *VP1.Vertex[2], mR1to0, mT1to0);
// Perform triangle-triangle overlap test
if(TriTriOverlap(*VP0.Vertex[0], *VP0.Vertex[1], *VP0.Vertex[2], u0, u1, u2))
{
// Keep track of colliding pairs
mPairs.Add(id0).Add(id1);
// Set contact status
mFlags |= OPC_CONTACT;
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Leaf-leaf test for a previously fetched triangle from tree A (in B's space) and a new leaf from B.
* \param id1 [in] leaf-triangle index from tree B
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ void AABBTreeCollider::PrimTestTriIndex(udword id1)
{
// Request vertices from the app
VertexPointers VP;
mIMesh1->GetTriangle(VP, id1);
// Perform triangle-triangle overlap test
if(TriTriOverlap(mLeafVerts[0], mLeafVerts[1], mLeafVerts[2], *VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2]))
{
// Keep track of colliding pairs
mPairs.Add(mLeafIndex).Add(id1);
// Set contact status
mFlags |= OPC_CONTACT;
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Leaf-leaf test for a previously fetched triangle from tree B (in A's space) and a new leaf from A.
* \param id0 [in] leaf-triangle index from tree A
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ void AABBTreeCollider::PrimTestIndexTri(udword id0)
{
// Request vertices from the app
VertexPointers VP;
mIMesh0->GetTriangle(VP, id0);
// Perform triangle-triangle overlap test
if(TriTriOverlap(mLeafVerts[0], mLeafVerts[1], mLeafVerts[2], *VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2]))
{
// Keep track of colliding pairs
mPairs.Add(id0).Add(mLeafIndex);
// Set contact status
mFlags |= OPC_CONTACT;
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision of a leaf node from A and a branch from B.
* \param b [in] collision node from second tree
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void AABBTreeCollider::_CollideTriBox(const AABBNoLeafNode* b)
{
// Perform triangle-box overlap test
if(!TriBoxOverlap(b->mAABB.mCenter, b->mAABB.mExtents)) return;
// Keep same triangle, deal with first child
if(b->HasPosLeaf()) PrimTestTriIndex(b->GetPosPrimitive());
else _CollideTriBox(b->GetPos());
if(ContactFound()) return;
// Keep same triangle, deal with second child
if(b->HasNegLeaf()) PrimTestTriIndex(b->GetNegPrimitive());
else _CollideTriBox(b->GetNeg());
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision of a leaf node from B and a branch from A.
* \param b [in] collision node from first tree
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void AABBTreeCollider::_CollideBoxTri(const AABBNoLeafNode* b)
{
// Perform triangle-box overlap test
if(!TriBoxOverlap(b->mAABB.mCenter, b->mAABB.mExtents)) return;
// Keep same triangle, deal with first child
if(b->HasPosLeaf()) PrimTestIndexTri(b->GetPosPrimitive());
else _CollideBoxTri(b->GetPos());
if(ContactFound()) return;
// Keep same triangle, deal with second child
if(b->HasNegLeaf()) PrimTestIndexTri(b->GetNegPrimitive());
else _CollideBoxTri(b->GetNeg());
}
//! Request triangle vertices from the app and transform them
#define FETCH_LEAF(prim_index, imesh, rot, trans) \
mLeafIndex = prim_index; \
/* Request vertices from the app */ \
VertexPointers VP; imesh->GetTriangle(VP, prim_index); \
/* Transform them in a common space */ \
TransformPoint(mLeafVerts[0], *VP.Vertex[0], rot, trans); \
TransformPoint(mLeafVerts[1], *VP.Vertex[1], rot, trans); \
TransformPoint(mLeafVerts[2], *VP.Vertex[2], rot, trans);
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for no-leaf AABB trees.
* \param a [in] collision node from first tree
* \param b [in] collision node from second tree
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void AABBTreeCollider::_Collide(const AABBNoLeafNode* a, const AABBNoLeafNode* b)
{
// Perform BV-BV overlap test
if(!BoxBoxOverlap(a->mAABB.mExtents, a->mAABB.mCenter, b->mAABB.mExtents, b->mAABB.mCenter)) return;
// Catch leaf status
BOOL BHasPosLeaf = b->HasPosLeaf();
BOOL BHasNegLeaf = b->HasNegLeaf();
if(a->HasPosLeaf())
{
FETCH_LEAF(a->GetPosPrimitive(), mIMesh0, mR0to1, mT0to1)
if(BHasPosLeaf) PrimTestTriIndex(b->GetPosPrimitive());
else _CollideTriBox(b->GetPos());
if(ContactFound()) return;
if(BHasNegLeaf) PrimTestTriIndex(b->GetNegPrimitive());
else _CollideTriBox(b->GetNeg());
}
else
{
if(BHasPosLeaf)
{
FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0)
_CollideBoxTri(a->GetPos());
}
else _Collide(a->GetPos(), b->GetPos());
if(ContactFound()) return;
if(BHasNegLeaf)
{
FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0)
_CollideBoxTri(a->GetPos());
}
else _Collide(a->GetPos(), b->GetNeg());
}
if(ContactFound()) return;
if(a->HasNegLeaf())
{
FETCH_LEAF(a->GetNegPrimitive(), mIMesh0, mR0to1, mT0to1)
if(BHasPosLeaf) PrimTestTriIndex(b->GetPosPrimitive());
else _CollideTriBox(b->GetPos());
if(ContactFound()) return;
if(BHasNegLeaf) PrimTestTriIndex(b->GetNegPrimitive());
else _CollideTriBox(b->GetNeg());
}
else
{
if(BHasPosLeaf)
{
// ### That leaf has possibly already been fetched
FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0)
_CollideBoxTri(a->GetNeg());
}
else _Collide(a->GetNeg(), b->GetPos());
if(ContactFound()) return;
if(BHasNegLeaf)
{
// ### That leaf has possibly already been fetched
FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0)
_CollideBoxTri(a->GetNeg());
}
else _Collide(a->GetNeg(), b->GetNeg());
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Quantized trees
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for quantized AABB trees.
* \param b0 [in] collision node from first tree
* \param b1 [in] collision node from second tree
* \param a [in] extent from box A
* \param Pa [in] center from box A
* \param b [in] extent from box B
* \param Pb [in] center from box B
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void AABBTreeCollider::_Collide(const AABBQuantizedNode* b0, const AABBQuantizedNode* b1, const Point& a, const Point& Pa, const Point& b, const Point& Pb)
{
// Perform BV-BV overlap test
if(!BoxBoxOverlap(a, Pa, b, Pb)) return;
if(b0->IsLeaf() && b1->IsLeaf()) { PrimTest(b0->GetPrimitive(), b1->GetPrimitive()); return; }
if(b1->IsLeaf() || (!b0->IsLeaf() && (b0->GetSize() > b1->GetSize())))
{
// Dequantize box
const QuantizedAABB* Box = &b0->GetNeg()->mAABB;
const Point negPa(float(Box->mCenter[0]) * mCenterCoeff0.x, float(Box->mCenter[1]) * mCenterCoeff0.y, float(Box->mCenter[2]) * mCenterCoeff0.z);
const Point nega(float(Box->mExtents[0]) * mExtentsCoeff0.x, float(Box->mExtents[1]) * mExtentsCoeff0.y, float(Box->mExtents[2]) * mExtentsCoeff0.z);
_Collide(b0->GetNeg(), b1, nega, negPa, b, Pb);
if(ContactFound()) return;
// Dequantize box
Box = &b0->GetPos()->mAABB;
const Point posPa(float(Box->mCenter[0]) * mCenterCoeff0.x, float(Box->mCenter[1]) * mCenterCoeff0.y, float(Box->mCenter[2]) * mCenterCoeff0.z);
const Point posa(float(Box->mExtents[0]) * mExtentsCoeff0.x, float(Box->mExtents[1]) * mExtentsCoeff0.y, float(Box->mExtents[2]) * mExtentsCoeff0.z);
_Collide(b0->GetPos(), b1, posa, posPa, b, Pb);
}
else
{
// Dequantize box
const QuantizedAABB* Box = &b1->GetNeg()->mAABB;
const Point negPb(float(Box->mCenter[0]) * mCenterCoeff1.x, float(Box->mCenter[1]) * mCenterCoeff1.y, float(Box->mCenter[2]) * mCenterCoeff1.z);
const Point negb(float(Box->mExtents[0]) * mExtentsCoeff1.x, float(Box->mExtents[1]) * mExtentsCoeff1.y, float(Box->mExtents[2]) * mExtentsCoeff1.z);
_Collide(b0, b1->GetNeg(), a, Pa, negb, negPb);
if(ContactFound()) return;
// Dequantize box
Box = &b1->GetPos()->mAABB;
const Point posPb(float(Box->mCenter[0]) * mCenterCoeff1.x, float(Box->mCenter[1]) * mCenterCoeff1.y, float(Box->mCenter[2]) * mCenterCoeff1.z);
const Point posb(float(Box->mExtents[0]) * mExtentsCoeff1.x, float(Box->mExtents[1]) * mExtentsCoeff1.y, float(Box->mExtents[2]) * mExtentsCoeff1.z);
_Collide(b0, b1->GetPos(), a, Pa, posb, posPb);
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Quantized no-leaf trees
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision of a leaf node from A and a quantized branch from B.
* \param leaf [in] leaf triangle from first tree
* \param b [in] collision node from second tree
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void AABBTreeCollider::_CollideTriBox(const AABBQuantizedNoLeafNode* b)
{
// Dequantize box
const QuantizedAABB* bb = &b->mAABB;
const Point Pb(float(bb->mCenter[0]) * mCenterCoeff1.x, float(bb->mCenter[1]) * mCenterCoeff1.y, float(bb->mCenter[2]) * mCenterCoeff1.z);
const Point eb(float(bb->mExtents[0]) * mExtentsCoeff1.x, float(bb->mExtents[1]) * mExtentsCoeff1.y, float(bb->mExtents[2]) * mExtentsCoeff1.z);
// Perform triangle-box overlap test
if(!TriBoxOverlap(Pb, eb)) return;
if(b->HasPosLeaf()) PrimTestTriIndex(b->GetPosPrimitive());
else _CollideTriBox(b->GetPos());
if(ContactFound()) return;
if(b->HasNegLeaf()) PrimTestTriIndex(b->GetNegPrimitive());
else _CollideTriBox(b->GetNeg());
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision of a leaf node from B and a quantized branch from A.
* \param b [in] collision node from first tree
* \param leaf [in] leaf triangle from second tree
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void AABBTreeCollider::_CollideBoxTri(const AABBQuantizedNoLeafNode* b)
{
// Dequantize box
const QuantizedAABB* bb = &b->mAABB;
const Point Pa(float(bb->mCenter[0]) * mCenterCoeff0.x, float(bb->mCenter[1]) * mCenterCoeff0.y, float(bb->mCenter[2]) * mCenterCoeff0.z);
const Point ea(float(bb->mExtents[0]) * mExtentsCoeff0.x, float(bb->mExtents[1]) * mExtentsCoeff0.y, float(bb->mExtents[2]) * mExtentsCoeff0.z);
// Perform triangle-box overlap test
if(!TriBoxOverlap(Pa, ea)) return;
if(b->HasPosLeaf()) PrimTestIndexTri(b->GetPosPrimitive());
else _CollideBoxTri(b->GetPos());
if(ContactFound()) return;
if(b->HasNegLeaf()) PrimTestIndexTri(b->GetNegPrimitive());
else _CollideBoxTri(b->GetNeg());
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for quantized no-leaf AABB trees.
* \param a [in] collision node from first tree
* \param b [in] collision node from second tree
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void AABBTreeCollider::_Collide(const AABBQuantizedNoLeafNode* a, const AABBQuantizedNoLeafNode* b)
{
// Dequantize box A
const QuantizedAABB* ab = &a->mAABB;
const Point Pa(float(ab->mCenter[0]) * mCenterCoeff0.x, float(ab->mCenter[1]) * mCenterCoeff0.y, float(ab->mCenter[2]) * mCenterCoeff0.z);
const Point ea(float(ab->mExtents[0]) * mExtentsCoeff0.x, float(ab->mExtents[1]) * mExtentsCoeff0.y, float(ab->mExtents[2]) * mExtentsCoeff0.z);
// Dequantize box B
const QuantizedAABB* bb = &b->mAABB;
const Point Pb(float(bb->mCenter[0]) * mCenterCoeff1.x, float(bb->mCenter[1]) * mCenterCoeff1.y, float(bb->mCenter[2]) * mCenterCoeff1.z);
const Point eb(float(bb->mExtents[0]) * mExtentsCoeff1.x, float(bb->mExtents[1]) * mExtentsCoeff1.y, float(bb->mExtents[2]) * mExtentsCoeff1.z);
// Perform BV-BV overlap test
if(!BoxBoxOverlap(ea, Pa, eb, Pb)) return;
// Catch leaf status
BOOL BHasPosLeaf = b->HasPosLeaf();
BOOL BHasNegLeaf = b->HasNegLeaf();
if(a->HasPosLeaf())
{
FETCH_LEAF(a->GetPosPrimitive(), mIMesh0, mR0to1, mT0to1)
if(BHasPosLeaf) PrimTestTriIndex(b->GetPosPrimitive());
else _CollideTriBox(b->GetPos());
if(ContactFound()) return;
if(BHasNegLeaf) PrimTestTriIndex(b->GetNegPrimitive());
else _CollideTriBox(b->GetNeg());
}
else
{
if(BHasPosLeaf)
{
FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0)
_CollideBoxTri(a->GetPos());
}
else _Collide(a->GetPos(), b->GetPos());
if(ContactFound()) return;
if(BHasNegLeaf)
{
FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0)
_CollideBoxTri(a->GetPos());
}
else _Collide(a->GetPos(), b->GetNeg());
}
if(ContactFound()) return;
if(a->HasNegLeaf())
{
FETCH_LEAF(a->GetNegPrimitive(), mIMesh0, mR0to1, mT0to1)
if(BHasPosLeaf) PrimTestTriIndex(b->GetPosPrimitive());
else _CollideTriBox(b->GetPos());
if(ContactFound()) return;
if(BHasNegLeaf) PrimTestTriIndex(b->GetNegPrimitive());
else _CollideTriBox(b->GetNeg());
}
else
{
if(BHasPosLeaf)
{
// ### That leaf has possibly already been fetched
FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0)
_CollideBoxTri(a->GetNeg());
}
else _Collide(a->GetNeg(), b->GetPos());
if(ContactFound()) return;
if(BHasNegLeaf)
{
// ### That leaf has possibly already been fetched
FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0)
_CollideBoxTri(a->GetNeg());
}
else _Collide(a->GetNeg(), b->GetNeg());
}
}

246
ode/OPCODE/OPC_TreeCollider.h Normal file
View File

@ -0,0 +1,246 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for a tree collider.
* \file OPC_TreeCollider.h
* \author Pierre Terdiman
* \date March, 20, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __OPC_TREECOLLIDER_H__
#define __OPC_TREECOLLIDER_H__
//! This structure holds cached information used by the algorithm.
//! Two model pointers and two colliding primitives are cached. Model pointers are assigned
//! to their respective meshes, and the pair of colliding primitives is used for temporal
//! coherence. That is, in case temporal coherence is enabled, those two primitives are
//! tested for overlap before everything else. If they still collide, we're done before
//! even entering the recursive collision code.
struct OPCODE_API BVTCache : Pair
{
//! Constructor
inline_ BVTCache()
{
ResetCache();
ResetCountDown();
}
void ResetCache()
{
Model0 = null;
Model1 = null;
id0 = 0;
id1 = 1;
#ifdef __MESHMERIZER_H__ // Collision hulls only supported within ICE !
HullTest = true;
SepVector.pid = 0;
SepVector.qid = 0;
SepVector.SV = Point(1.0f, 0.0f, 0.0f);
#endif // __MESHMERIZER_H__
}
#ifdef __MESHMERIZER_H__ // Collision hulls only supported within ICE !
inline_ void ResetCountDown()
{
CountDown = 50;
}
#else
void ResetCountDown(){};
#endif // __MESHMERIZER_H__
const Model* Model0; //!< Model for first object
const Model* Model1; //!< Model for second object
#ifdef __MESHMERIZER_H__ // Collision hulls only supported within ICE !
SVCache SepVector;
udword CountDown;
bool HullTest;
#endif // __MESHMERIZER_H__
};
class OPCODE_API AABBTreeCollider : public Collider
{
public:
// Constructor / Destructor
AABBTreeCollider();
virtual ~AABBTreeCollider();
// Generic collision query
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Generic collision query for generic OPCODE models. After the call, access the results with:
* - GetContactStatus()
* - GetNbPairs()
* - GetPairs()
*
* \param cache [in] collision cache for model pointers and a colliding pair of primitives
* \param world0 [in] world matrix for first object, or null
* \param world1 [in] world matrix for second object, or null
* \return true if success
* \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool Collide(BVTCache& cache, const Matrix4x4* world0=null, const Matrix4x4* world1=null);
// Collision queries
bool Collide(const AABBCollisionTree* tree0, const AABBCollisionTree* tree1, const Matrix4x4* world0=null, const Matrix4x4* world1=null, Pair* cache=null);
bool Collide(const AABBNoLeafTree* tree0, const AABBNoLeafTree* tree1, const Matrix4x4* world0=null, const Matrix4x4* world1=null, Pair* cache=null);
bool Collide(const AABBQuantizedTree* tree0, const AABBQuantizedTree* tree1, const Matrix4x4* world0=null, const Matrix4x4* world1=null, Pair* cache=null);
bool Collide(const AABBQuantizedNoLeafTree* tree0, const AABBQuantizedNoLeafTree* tree1, const Matrix4x4* world0=null, const Matrix4x4* world1=null, Pair* cache=null);
// Settings
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Settings: selects between full box-box tests or "SAT-lite" tests (where Class III axes are discarded)
* \param flag [in] true for full tests, false for coarse tests
* \see SetFullPrimBoxTest(bool flag)
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ void SetFullBoxBoxTest(bool flag) { mFullBoxBoxTest = flag; }
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Settings: selects between full triangle-box tests or "SAT-lite" tests (where Class III axes are discarded)
* \param flag [in] true for full tests, false for coarse tests
* \see SetFullBoxBoxTest(bool flag)
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ void SetFullPrimBoxTest(bool flag) { mFullPrimBoxTest = flag; }
// Stats
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Stats: gets the number of BV-BV overlap tests after a collision query.
* \see GetNbPrimPrimTests()
* \see GetNbBVPrimTests()
* \return the number of BV-BV tests performed during last query
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ udword GetNbBVBVTests() const { return mNbBVBVTests; }
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Stats: gets the number of Triangle-Triangle overlap tests after a collision query.
* \see GetNbBVBVTests()
* \see GetNbBVPrimTests()
* \return the number of Triangle-Triangle tests performed during last query
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ udword GetNbPrimPrimTests() const { return mNbPrimPrimTests; }
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Stats: gets the number of BV-Triangle overlap tests after a collision query.
* \see GetNbBVBVTests()
* \see GetNbPrimPrimTests()
* \return the number of BV-Triangle tests performed during last query
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ udword GetNbBVPrimTests() const { return mNbBVPrimTests; }
// Data access
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Gets the number of contacts after a collision query.
* \see GetContactStatus()
* \see GetPairs()
* \return the number of contacts / colliding pairs.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ udword GetNbPairs() const { return mPairs.GetNbEntries()>>1; }
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Gets the pairs of colliding triangles after a collision query.
* \see GetContactStatus()
* \see GetNbPairs()
* \return the list of colliding pairs (triangle indices)
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ const Pair* GetPairs() const { return (const Pair*)mPairs.GetEntries(); }
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Validates current settings. You should call this method after all the settings and callbacks have been defined for a collider.
* \return null if everything is ok, else a string describing the problem
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
override(Collider) const char* ValidateSettings();
protected:
// Colliding pairs
Container mPairs; //!< Pairs of colliding primitives
// User mesh interfaces
const MeshInterface* mIMesh0; //!< User-defined mesh interface for object0
const MeshInterface* mIMesh1; //!< User-defined mesh interface for object1
// Stats
udword mNbBVBVTests; //!< Number of BV-BV tests
udword mNbPrimPrimTests; //!< Number of Primitive-Primitive tests
udword mNbBVPrimTests; //!< Number of BV-Primitive tests
// Precomputed data
Matrix3x3 mAR; //!< Absolute rotation matrix
Matrix3x3 mR0to1; //!< Rotation from object0 to object1
Matrix3x3 mR1to0; //!< Rotation from object1 to object0
Point mT0to1; //!< Translation from object0 to object1
Point mT1to0; //!< Translation from object1 to object0
// Dequantization coeffs
Point mCenterCoeff0;
Point mExtentsCoeff0;
Point mCenterCoeff1;
Point mExtentsCoeff1;
// Leaf description
Point mLeafVerts[3]; //!< Triangle vertices
udword mLeafIndex; //!< Triangle index
// Settings
bool mFullBoxBoxTest; //!< Perform full BV-BV tests (true) or SAT-lite tests (false)
bool mFullPrimBoxTest; //!< Perform full Primitive-BV tests (true) or SAT-lite tests (false)
// Internal methods
// Standard AABB trees
void _Collide(const AABBCollisionNode* b0, const AABBCollisionNode* b1);
// Quantized AABB trees
void _Collide(const AABBQuantizedNode* b0, const AABBQuantizedNode* b1, const Point& a, const Point& Pa, const Point& b, const Point& Pb);
// No-leaf AABB trees
void _CollideTriBox(const AABBNoLeafNode* b);
void _CollideBoxTri(const AABBNoLeafNode* b);
void _Collide(const AABBNoLeafNode* a, const AABBNoLeafNode* b);
// Quantized no-leaf AABB trees
void _CollideTriBox(const AABBQuantizedNoLeafNode* b);
void _CollideBoxTri(const AABBQuantizedNoLeafNode* b);
void _Collide(const AABBQuantizedNoLeafNode* a, const AABBQuantizedNoLeafNode* b);
// Overlap tests
void PrimTest(udword id0, udword id1);
inline_ void PrimTestTriIndex(udword id1);
inline_ void PrimTestIndexTri(udword id0);
inline_ BOOL BoxBoxOverlap(const Point& ea, const Point& ca, const Point& eb, const Point& cb);
inline_ BOOL TriBoxOverlap(const Point& center, const Point& extents);
inline_ BOOL TriTriOverlap(const Point& V0, const Point& V1, const Point& V2, const Point& U0, const Point& U1, const Point& U2);
// Init methods
void InitQuery(const Matrix4x4* world0=null, const Matrix4x4* world1=null);
bool CheckTemporalCoherence(Pair* cache);
inline_ BOOL Setup(const MeshInterface* mi0, const MeshInterface* mi1)
{
mIMesh0 = mi0;
mIMesh1 = mi1;
if(!mIMesh0 || !mIMesh1) return FALSE;
return TRUE;
}
};
#endif // __OPC_TREECOLLIDER_H__

339
ode/OPCODE/OPC_TriBoxOverlap.h Normal file
View File

@ -0,0 +1,339 @@
//! This macro quickly finds the min & max values among 3 variables
#define FINDMINMAX(x0, x1, x2, min, max) \
min = max = x0; \
if(x1<min) min=x1; \
if(x1>max) max=x1; \
if(x2<min) min=x2; \
if(x2>max) max=x2;
//! TO BE DOCUMENTED
inline_ BOOL planeBoxOverlap(const Point& normal, const float d, const Point& maxbox)
{
Point vmin, vmax;
for(udword q=0;q<=2;q++)
{
if(normal[q]>0.0f) { vmin[q]=-maxbox[q]; vmax[q]=maxbox[q]; }
else { vmin[q]=maxbox[q]; vmax[q]=-maxbox[q]; }
}
if((normal|vmin)+d>0.0f) return FALSE;
if((normal|vmax)+d>=0.0f) return TRUE;
return FALSE;
}
//! TO BE DOCUMENTED
#define AXISTEST_X01(a, b, fa, fb) \
min = a*v0.y - b*v0.z; \
max = a*v2.y - b*v2.z; \
if(min>max) {const float tmp=max; max=min; min=tmp; } \
rad = fa * extents.y + fb * extents.z; \
if(min>rad || max<-rad) return FALSE;
//! TO BE DOCUMENTED
#define AXISTEST_X2(a, b, fa, fb) \
min = a*v0.y - b*v0.z; \
max = a*v1.y - b*v1.z; \
if(min>max) {const float tmp=max; max=min; min=tmp; } \
rad = fa * extents.y + fb * extents.z; \
if(min>rad || max<-rad) return FALSE;
//! TO BE DOCUMENTED
#define AXISTEST_Y02(a, b, fa, fb) \
min = b*v0.z - a*v0.x; \
max = b*v2.z - a*v2.x; \
if(min>max) {const float tmp=max; max=min; min=tmp; } \
rad = fa * extents.x + fb * extents.z; \
if(min>rad || max<-rad) return FALSE;
//! TO BE DOCUMENTED
#define AXISTEST_Y1(a, b, fa, fb) \
min = b*v0.z - a*v0.x; \
max = b*v1.z - a*v1.x; \
if(min>max) {const float tmp=max; max=min; min=tmp; } \
rad = fa * extents.x + fb * extents.z; \
if(min>rad || max<-rad) return FALSE;
//! TO BE DOCUMENTED
#define AXISTEST_Z12(a, b, fa, fb) \
min = a*v1.x - b*v1.y; \
max = a*v2.x - b*v2.y; \
if(min>max) {const float tmp=max; max=min; min=tmp; } \
rad = fa * extents.x + fb * extents.y; \
if(min>rad || max<-rad) return FALSE;
//! TO BE DOCUMENTED
#define AXISTEST_Z0(a, b, fa, fb) \
min = a*v0.x - b*v0.y; \
max = a*v1.x - b*v1.y; \
if(min>max) {const float tmp=max; max=min; min=tmp; } \
rad = fa * extents.x + fb * extents.y; \
if(min>rad || max<-rad) return FALSE;
// compute triangle edges
// - edges lazy evaluated to take advantage of early exits
// - fabs precomputed (half less work, possible since extents are always >0)
// - customized macros to take advantage of the null component
// - axis vector discarded, possibly saves useless movs
#define IMPLEMENT_CLASS3_TESTS \
float rad; \
float min, max; \
\
const float fey0 = fabsf(e0.y); \
const float fez0 = fabsf(e0.z); \
AXISTEST_X01(e0.z, e0.y, fez0, fey0); \
const float fex0 = fabsf(e0.x); \
AXISTEST_Y02(e0.z, e0.x, fez0, fex0); \
AXISTEST_Z12(e0.y, e0.x, fey0, fex0); \
\
const float fey1 = fabsf(e1.y); \
const float fez1 = fabsf(e1.z); \
AXISTEST_X01(e1.z, e1.y, fez1, fey1); \
const float fex1 = fabsf(e1.x); \
AXISTEST_Y02(e1.z, e1.x, fez1, fex1); \
AXISTEST_Z0(e1.y, e1.x, fey1, fex1); \
\
const Point e2 = mLeafVerts[0] - mLeafVerts[2]; \
const float fey2 = fabsf(e2.y); \
const float fez2 = fabsf(e2.z); \
AXISTEST_X2(e2.z, e2.y, fez2, fey2); \
const float fex2 = fabsf(e2.x); \
AXISTEST_Y1(e2.z, e2.x, fez2, fex2); \
AXISTEST_Z12(e2.y, e2.x, fey2, fex2);
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Triangle-Box overlap test using the separating axis theorem.
* This is the code from Tomas M<>ller, a bit optimized:
* - with some more lazy evaluation (faster path on PC)
* - with a tiny bit of assembly
* - with "SAT-lite" applied if needed
* - and perhaps with some more minor modifs...
*
* \param center [in] box center
* \param extents [in] box extents
* \return true if triangle & box overlap
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ BOOL AABBTreeCollider::TriBoxOverlap(const Point& center, const Point& extents)
{
// Stats
mNbBVPrimTests++;
// use separating axis theorem to test overlap between triangle and box
// need to test for overlap in these directions:
// 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle
// we do not even need to test these)
// 2) normal of the triangle
// 3) crossproduct(edge from tri, {x,y,z}-directin)
// this gives 3x3=9 more tests
// move everything so that the boxcenter is in (0,0,0)
Point v0, v1, v2;
v0.x = mLeafVerts[0].x - center.x;
v1.x = mLeafVerts[1].x - center.x;
v2.x = mLeafVerts[2].x - center.x;
// First, test overlap in the {x,y,z}-directions
#ifdef OPC_USE_FCOMI
// find min, max of the triangle in x-direction, and test for overlap in X
if(FCMin3(v0.x, v1.x, v2.x)>extents.x) return FALSE;
if(FCMax3(v0.x, v1.x, v2.x)<-extents.x) return FALSE;
// same for Y
v0.y = mLeafVerts[0].y - center.y;
v1.y = mLeafVerts[1].y - center.y;
v2.y = mLeafVerts[2].y - center.y;
if(FCMin3(v0.y, v1.y, v2.y)>extents.y) return FALSE;
if(FCMax3(v0.y, v1.y, v2.y)<-extents.y) return FALSE;
// same for Z
v0.z = mLeafVerts[0].z - center.z;
v1.z = mLeafVerts[1].z - center.z;
v2.z = mLeafVerts[2].z - center.z;
if(FCMin3(v0.z, v1.z, v2.z)>extents.z) return FALSE;
if(FCMax3(v0.z, v1.z, v2.z)<-extents.z) return FALSE;
#else
float min,max;
// Find min, max of the triangle in x-direction, and test for overlap in X
FINDMINMAX(v0.x, v1.x, v2.x, min, max);
if(min>extents.x || max<-extents.x) return FALSE;
// Same for Y
v0.y = mLeafVerts[0].y - center.y;
v1.y = mLeafVerts[1].y - center.y;
v2.y = mLeafVerts[2].y - center.y;
FINDMINMAX(v0.y, v1.y, v2.y, min, max);
if(min>extents.y || max<-extents.y) return FALSE;
// Same for Z
v0.z = mLeafVerts[0].z - center.z;
v1.z = mLeafVerts[1].z - center.z;
v2.z = mLeafVerts[2].z - center.z;
FINDMINMAX(v0.z, v1.z, v2.z, min, max);
if(min>extents.z || max<-extents.z) return FALSE;
#endif
// 2) Test if the box intersects the plane of the triangle
// compute plane equation of triangle: normal*x+d=0
// ### could be precomputed since we use the same leaf triangle several times
const Point e0 = v1 - v0;
const Point e1 = v2 - v1;
const Point normal = e0 ^ e1;
const float d = -normal|v0;
if(!planeBoxOverlap(normal, d, extents)) return FALSE;
// 3) "Class III" tests
if(mFullPrimBoxTest)
{
IMPLEMENT_CLASS3_TESTS
}
return TRUE;
}
//! A dedicated version where the box is constant
inline_ BOOL OBBCollider::TriBoxOverlap()
{
// Stats
mNbVolumePrimTests++;
// Hook
const Point& extents = mBoxExtents;
const Point& v0 = mLeafVerts[0];
const Point& v1 = mLeafVerts[1];
const Point& v2 = mLeafVerts[2];
// use separating axis theorem to test overlap between triangle and box
// need to test for overlap in these directions:
// 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle
// we do not even need to test these)
// 2) normal of the triangle
// 3) crossproduct(edge from tri, {x,y,z}-directin)
// this gives 3x3=9 more tests
// Box center is already in (0,0,0)
// First, test overlap in the {x,y,z}-directions
#ifdef OPC_USE_FCOMI
// find min, max of the triangle in x-direction, and test for overlap in X
if(FCMin3(v0.x, v1.x, v2.x)>mBoxExtents.x) return FALSE;
if(FCMax3(v0.x, v1.x, v2.x)<-mBoxExtents.x) return FALSE;
if(FCMin3(v0.y, v1.y, v2.y)>mBoxExtents.y) return FALSE;
if(FCMax3(v0.y, v1.y, v2.y)<-mBoxExtents.y) return FALSE;
if(FCMin3(v0.z, v1.z, v2.z)>mBoxExtents.z) return FALSE;
if(FCMax3(v0.z, v1.z, v2.z)<-mBoxExtents.z) return FALSE;
#else
float min,max;
// Find min, max of the triangle in x-direction, and test for overlap in X
FINDMINMAX(v0.x, v1.x, v2.x, min, max);
if(min>mBoxExtents.x || max<-mBoxExtents.x) return FALSE;
FINDMINMAX(v0.y, v1.y, v2.y, min, max);
if(min>mBoxExtents.y || max<-mBoxExtents.y) return FALSE;
FINDMINMAX(v0.z, v1.z, v2.z, min, max);
if(min>mBoxExtents.z || max<-mBoxExtents.z) return FALSE;
#endif
// 2) Test if the box intersects the plane of the triangle
// compute plane equation of triangle: normal*x+d=0
// ### could be precomputed since we use the same leaf triangle several times
const Point e0 = v1 - v0;
const Point e1 = v2 - v1;
const Point normal = e0 ^ e1;
const float d = -normal|v0;
if(!planeBoxOverlap(normal, d, mBoxExtents)) return FALSE;
// 3) "Class III" tests - here we always do full tests since the box is a primitive (not a BV)
{
IMPLEMENT_CLASS3_TESTS
}
return TRUE;
}
//! ...and another one, jeez
inline_ BOOL AABBCollider::TriBoxOverlap()
{
// Stats
mNbVolumePrimTests++;
// Hook
const Point& center = mBox.mCenter;
const Point& extents = mBox.mExtents;
// use separating axis theorem to test overlap between triangle and box
// need to test for overlap in these directions:
// 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle
// we do not even need to test these)
// 2) normal of the triangle
// 3) crossproduct(edge from tri, {x,y,z}-directin)
// this gives 3x3=9 more tests
// move everything so that the boxcenter is in (0,0,0)
Point v0, v1, v2;
v0.x = mLeafVerts[0].x - center.x;
v1.x = mLeafVerts[1].x - center.x;
v2.x = mLeafVerts[2].x - center.x;
// First, test overlap in the {x,y,z}-directions
#ifdef OPC_USE_FCOMI
// find min, max of the triangle in x-direction, and test for overlap in X
if(FCMin3(v0.x, v1.x, v2.x)>extents.x) return FALSE;
if(FCMax3(v0.x, v1.x, v2.x)<-extents.x) return FALSE;
// same for Y
v0.y = mLeafVerts[0].y - center.y;
v1.y = mLeafVerts[1].y - center.y;
v2.y = mLeafVerts[2].y - center.y;
if(FCMin3(v0.y, v1.y, v2.y)>extents.y) return FALSE;
if(FCMax3(v0.y, v1.y, v2.y)<-extents.y) return FALSE;
// same for Z
v0.z = mLeafVerts[0].z - center.z;
v1.z = mLeafVerts[1].z - center.z;
v2.z = mLeafVerts[2].z - center.z;
if(FCMin3(v0.z, v1.z, v2.z)>extents.z) return FALSE;
if(FCMax3(v0.z, v1.z, v2.z)<-extents.z) return FALSE;
#else
float min,max;
// Find min, max of the triangle in x-direction, and test for overlap in X
FINDMINMAX(v0.x, v1.x, v2.x, min, max);
if(min>extents.x || max<-extents.x) return FALSE;
// Same for Y
v0.y = mLeafVerts[0].y - center.y;
v1.y = mLeafVerts[1].y - center.y;
v2.y = mLeafVerts[2].y - center.y;
FINDMINMAX(v0.y, v1.y, v2.y, min, max);
if(min>extents.y || max<-extents.y) return FALSE;
// Same for Z
v0.z = mLeafVerts[0].z - center.z;
v1.z = mLeafVerts[1].z - center.z;
v2.z = mLeafVerts[2].z - center.z;
FINDMINMAX(v0.z, v1.z, v2.z, min, max);
if(min>extents.z || max<-extents.z) return FALSE;
#endif
// 2) Test if the box intersects the plane of the triangle
// compute plane equation of triangle: normal*x+d=0
// ### could be precomputed since we use the same leaf triangle several times
const Point e0 = v1 - v0;
const Point e1 = v2 - v1;
const Point normal = e0 ^ e1;
const float d = -normal|v0;
if(!planeBoxOverlap(normal, d, extents)) return FALSE;
// 3) "Class III" tests - here we always do full tests since the box is a primitive (not a BV)
{
IMPLEMENT_CLASS3_TESTS
}
return TRUE;
}

279
ode/OPCODE/OPC_TriTriOverlap.h Normal file
View File

@ -0,0 +1,279 @@
//! if OPC_TRITRI_EPSILON_TEST is true then we do a check (if |dv|<EPSILON then dv=0.0;) else no check is done (which is less robust, but faster)
#define LOCAL_EPSILON 0.000001f
//! sort so that a<=b
#define SORT(a,b) \
if(a>b) \
{ \
const float c=a; \
a=b; \
b=c; \
}
//! Edge to edge test based on Franlin Antonio's gem: "Faster Line Segment Intersection", in Graphics Gems III, pp. 199-202
#define EDGE_EDGE_TEST(V0, U0, U1) \
Bx = U0[i0] - U1[i0]; \
By = U0[i1] - U1[i1]; \
Cx = V0[i0] - U0[i0]; \
Cy = V0[i1] - U0[i1]; \
f = Ay*Bx - Ax*By; \
d = By*Cx - Bx*Cy; \
if((f>0.0f && d>=0.0f && d<=f) || (f<0.0f && d<=0.0f && d>=f)) \
{ \
const float e=Ax*Cy - Ay*Cx; \
if(f>0.0f) \
{ \
if(e>=0.0f && e<=f) return TRUE; \
} \
else \
{ \
if(e<=0.0f && e>=f) return TRUE; \
} \
}
//! TO BE DOCUMENTED
#define EDGE_AGAINST_TRI_EDGES(V0, V1, U0, U1, U2) \
{ \
float Bx,By,Cx,Cy,d,f; \
const float Ax = V1[i0] - V0[i0]; \
const float Ay = V1[i1] - V0[i1]; \
/* test edge U0,U1 against V0,V1 */ \
EDGE_EDGE_TEST(V0, U0, U1); \
/* test edge U1,U2 against V0,V1 */ \
EDGE_EDGE_TEST(V0, U1, U2); \
/* test edge U2,U1 against V0,V1 */ \
EDGE_EDGE_TEST(V0, U2, U0); \
}
//! TO BE DOCUMENTED
#define POINT_IN_TRI(V0, U0, U1, U2) \
{ \
/* is T1 completly inside T2? */ \
/* check if V0 is inside tri(U0,U1,U2) */ \
float a = U1[i1] - U0[i1]; \
float b = -(U1[i0] - U0[i0]); \
float c = -a*U0[i0] - b*U0[i1]; \
float d0 = a*V0[i0] + b*V0[i1] + c; \
\
a = U2[i1] - U1[i1]; \
b = -(U2[i0] - U1[i0]); \
c = -a*U1[i0] - b*U1[i1]; \
const float d1 = a*V0[i0] + b*V0[i1] + c; \
\
a = U0[i1] - U2[i1]; \
b = -(U0[i0] - U2[i0]); \
c = -a*U2[i0] - b*U2[i1]; \
const float d2 = a*V0[i0] + b*V0[i1] + c; \
if(d0*d1>0.0f) \
{ \
if(d0*d2>0.0f) return TRUE; \
} \
}
//! TO BE DOCUMENTED
BOOL CoplanarTriTri(const Point& n, const Point& v0, const Point& v1, const Point& v2, const Point& u0, const Point& u1, const Point& u2)
{
float A[3];
short i0,i1;
/* first project onto an axis-aligned plane, that maximizes the area */
/* of the triangles, compute indices: i0,i1. */
A[0] = fabsf(n[0]);
A[1] = fabsf(n[1]);
A[2] = fabsf(n[2]);
if(A[0]>A[1])
{
if(A[0]>A[2])
{
i0=1; /* A[0] is greatest */
i1=2;
}
else
{
i0=0; /* A[2] is greatest */
i1=1;
}
}
else /* A[0]<=A[1] */
{
if(A[2]>A[1])
{
i0=0; /* A[2] is greatest */
i1=1;
}
else
{
i0=0; /* A[1] is greatest */
i1=2;
}
}
/* test all edges of triangle 1 against the edges of triangle 2 */
EDGE_AGAINST_TRI_EDGES(v0, v1, u0, u1, u2);
EDGE_AGAINST_TRI_EDGES(v1, v2, u0, u1, u2);
EDGE_AGAINST_TRI_EDGES(v2, v0, u0, u1, u2);
/* finally, test if tri1 is totally contained in tri2 or vice versa */
POINT_IN_TRI(v0, u0, u1, u2);
POINT_IN_TRI(u0, v0, v1, v2);
return FALSE;
}
//! TO BE DOCUMENTED
#define NEWCOMPUTE_INTERVALS(VV0, VV1, VV2, D0, D1, D2, D0D1, D0D2, A, B, C, X0, X1) \
{ \
if(D0D1>0.0f) \
{ \
/* here we know that D0D2<=0.0 */ \
/* that is D0, D1 are on the same side, D2 on the other or on the plane */ \
A=VV2; B=(VV0 - VV2)*D2; C=(VV1 - VV2)*D2; X0=D2 - D0; X1=D2 - D1; \
} \
else if(D0D2>0.0f) \
{ \
/* here we know that d0d1<=0.0 */ \
A=VV1; B=(VV0 - VV1)*D1; C=(VV2 - VV1)*D1; X0=D1 - D0; X1=D1 - D2; \
} \
else if(D1*D2>0.0f || D0!=0.0f) \
{ \
/* here we know that d0d1<=0.0 or that D0!=0.0 */ \
A=VV0; B=(VV1 - VV0)*D0; C=(VV2 - VV0)*D0; X0=D0 - D1; X1=D0 - D2; \
} \
else if(D1!=0.0f) \
{ \
A=VV1; B=(VV0 - VV1)*D1; C=(VV2 - VV1)*D1; X0=D1 - D0; X1=D1 - D2; \
} \
else if(D2!=0.0f) \
{ \
A=VV2; B=(VV0 - VV2)*D2; C=(VV1 - VV2)*D2; X0=D2 - D0; X1=D2 - D1; \
} \
else \
{ \
/* triangles are coplanar */ \
return CoplanarTriTri(N1, V0, V1, V2, U0, U1, U2); \
} \
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Triangle/triangle intersection test routine,
* by Tomas Moller, 1997.
* See article "A Fast Triangle-Triangle Intersection Test",
* Journal of Graphics Tools, 2(2), 1997
*
* Updated June 1999: removed the divisions -- a little faster now!
* Updated October 1999: added {} to CROSS and SUB macros
*
* int NoDivTriTriIsect(float V0[3],float V1[3],float V2[3],
* float U0[3],float U1[3],float U2[3])
*
* \param V0 [in] triangle 0, vertex 0
* \param V1 [in] triangle 0, vertex 1
* \param V2 [in] triangle 0, vertex 2
* \param U0 [in] triangle 1, vertex 0
* \param U1 [in] triangle 1, vertex 1
* \param U2 [in] triangle 1, vertex 2
* \return true if triangles overlap
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ BOOL AABBTreeCollider::TriTriOverlap(const Point& V0, const Point& V1, const Point& V2, const Point& U0, const Point& U1, const Point& U2)
{
// Stats
mNbPrimPrimTests++;
// Compute plane equation of triangle(V0,V1,V2)
Point E1 = V1 - V0;
Point E2 = V2 - V0;
const Point N1 = E1 ^ E2;
const float d1 =-N1 | V0;
// Plane equation 1: N1.X+d1=0
// Put U0,U1,U2 into plane equation 1 to compute signed distances to the plane
float du0 = (N1|U0) + d1;
float du1 = (N1|U1) + d1;
float du2 = (N1|U2) + d1;
// Coplanarity robustness check
#ifdef OPC_TRITRI_EPSILON_TEST
if(fabsf(du0)<LOCAL_EPSILON) du0 = 0.0f;
if(fabsf(du1)<LOCAL_EPSILON) du1 = 0.0f;
if(fabsf(du2)<LOCAL_EPSILON) du2 = 0.0f;
#endif
const float du0du1 = du0 * du1;
const float du0du2 = du0 * du2;
if(du0du1>0.0f && du0du2>0.0f) // same sign on all of them + not equal 0 ?
return FALSE; // no intersection occurs
// Compute plane of triangle (U0,U1,U2)
E1 = U1 - U0;
E2 = U2 - U0;
const Point N2 = E1 ^ E2;
const float d2=-N2 | U0;
// plane equation 2: N2.X+d2=0
// put V0,V1,V2 into plane equation 2
float dv0 = (N2|V0) + d2;
float dv1 = (N2|V1) + d2;
float dv2 = (N2|V2) + d2;
#ifdef OPC_TRITRI_EPSILON_TEST
if(fabsf(dv0)<LOCAL_EPSILON) dv0 = 0.0f;
if(fabsf(dv1)<LOCAL_EPSILON) dv1 = 0.0f;
if(fabsf(dv2)<LOCAL_EPSILON) dv2 = 0.0f;
#endif
const float dv0dv1 = dv0 * dv1;
const float dv0dv2 = dv0 * dv2;
if(dv0dv1>0.0f && dv0dv2>0.0f) // same sign on all of them + not equal 0 ?
return FALSE; // no intersection occurs
// Compute direction of intersection line
const Point D = N1^N2;
// Compute and index to the largest component of D
float max=fabsf(D[0]);
short index=0;
float bb=fabsf(D[1]);
float cc=fabsf(D[2]);
if(bb>max) max=bb,index=1;
if(cc>max) max=cc,index=2;
// This is the simplified projection onto L
const float vp0 = V0[index];
const float vp1 = V1[index];
const float vp2 = V2[index];
const float up0 = U0[index];
const float up1 = U1[index];
const float up2 = U2[index];
// Compute interval for triangle 1
float a,b,c,x0,x1;
NEWCOMPUTE_INTERVALS(vp0,vp1,vp2,dv0,dv1,dv2,dv0dv1,dv0dv2,a,b,c,x0,x1);
// Compute interval for triangle 2
float d,e,f,y0,y1;
NEWCOMPUTE_INTERVALS(up0,up1,up2,du0,du1,du2,du0du1,du0du2,d,e,f,y0,y1);
const float xx=x0*x1;
const float yy=y0*y1;
const float xxyy=xx*yy;
float isect1[2], isect2[2];
float tmp=a*xxyy;
isect1[0]=tmp+b*x1*yy;
isect1[1]=tmp+c*x0*yy;
tmp=d*xxyy;
isect2[0]=tmp+e*xx*y1;
isect2[1]=tmp+f*xx*y0;
SORT(isect1[0],isect1[1]);
SORT(isect2[0],isect2[1]);
if(isect1[1]<isect2[0] || isect2[1]<isect1[0]) return FALSE;
return TRUE;
}

103
ode/OPCODE/OPC_VolumeCollider.cpp Normal file
View File

@ -0,0 +1,103 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains base volume collider class.
* \file OPC_VolumeCollider.cpp
* \author Pierre Terdiman
* \date June, 2, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains the abstract class for volume colliders.
*
* \class VolumeCollider
* \author Pierre Terdiman
* \version 1.3
* \date June, 2, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Precompiled Header
#include "Stdafx.h"
using namespace Opcode;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Constructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
VolumeCollider::VolumeCollider() :
mTouchedPrimitives (null),
mNbVolumeBVTests (0),
mNbVolumePrimTests (0)
{
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Destructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
VolumeCollider::~VolumeCollider()
{
mTouchedPrimitives = null;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Validates current settings. You should call this method after all the settings / callbacks have been defined for a collider.
* \return null if everything is ok, else a string describing the problem
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
const char* VolumeCollider::ValidateSettings()
{
return null;
}
// Pretty dumb way to dump - to do better - one day...
#define IMPLEMENT_NOLEAFDUMP(type) \
void VolumeCollider::_Dump(const type* node) \
{ \
if(node->HasPosLeaf()) mTouchedPrimitives->Add(udword(node->GetPosPrimitive())); \
else _Dump(node->GetPos()); \
\
if(ContactFound()) return; \
\
if(node->HasNegLeaf()) mTouchedPrimitives->Add(udword(node->GetNegPrimitive())); \
else _Dump(node->GetNeg()); \
}
#define IMPLEMENT_LEAFDUMP(type) \
void VolumeCollider::_Dump(const type* node) \
{ \
if(node->IsLeaf()) \
{ \
mTouchedPrimitives->Add(udword(node->GetPrimitive())); \
} \
else \
{ \
_Dump(node->GetPos()); \
\
if(ContactFound()) return; \
\
_Dump(node->GetNeg()); \
} \
}
IMPLEMENT_NOLEAFDUMP(AABBNoLeafNode)
IMPLEMENT_NOLEAFDUMP(AABBQuantizedNoLeafNode)
IMPLEMENT_LEAFDUMP(AABBCollisionNode)
IMPLEMENT_LEAFDUMP(AABBQuantizedNode)

138
ode/OPCODE/OPC_VolumeCollider.h Normal file
View File

@ -0,0 +1,138 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains base volume collider class.
* \file OPC_VolumeCollider.h
* \author Pierre Terdiman
* \date June, 2, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __OPC_VOLUMECOLLIDER_H__
#define __OPC_VOLUMECOLLIDER_H__
struct OPCODE_API VolumeCache
{
VolumeCache() : Model(null) {}
~VolumeCache() {}
Container TouchedPrimitives; //!< Indices of touched primitives
const BaseModel* Model; //!< Owner
};
class OPCODE_API VolumeCollider : public Collider
{
public:
// Constructor / Destructor
VolumeCollider();
virtual ~VolumeCollider() = 0;
// Collision report
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Gets the number of touched primitives after a collision query.
* \see GetContactStatus()
* \see GetTouchedPrimitives()
* \return the number of touched primitives
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ udword GetNbTouchedPrimitives() const { return mTouchedPrimitives ? mTouchedPrimitives->GetNbEntries() : 0; }
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Gets the list of touched primitives after a collision query.
* \see GetContactStatus()
* \see GetNbTouchedPrimitives()
* \return the list of touched primitives (primitive indices)
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ const udword* GetTouchedPrimitives() const { return mTouchedPrimitives ? mTouchedPrimitives->GetEntries() : null; }
// Stats
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Stats: gets the number of Volume-BV overlap tests after a collision query.
* \see GetNbVolumePrimTests()
* \return the number of Volume-BV tests performed during last query
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ udword GetNbVolumeBVTests() const { return mNbVolumeBVTests; }
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Stats: gets the number of Volume-Triangle overlap tests after a collision query.
* \see GetNbVolumeBVTests()
* \return the number of Volume-Triangle tests performed during last query
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ udword GetNbVolumePrimTests() const { return mNbVolumePrimTests; }
// Settings
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Validates current settings. You should call this method after all the settings / callbacks have been defined for a collider.
* \return null if everything is ok, else a string describing the problem
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
override(Collider) const char* ValidateSettings();
protected:
// Touched primitives
Container* mTouchedPrimitives; //!< List of touched primitives
// Dequantization coeffs
Point mCenterCoeff;
Point mExtentsCoeff;
// Stats
udword mNbVolumeBVTests; //!< Number of Volume-BV tests
udword mNbVolumePrimTests; //!< Number of Volume-Primitive tests
// Internal methods
void _Dump(const AABBCollisionNode* node);
void _Dump(const AABBNoLeafNode* node);
void _Dump(const AABBQuantizedNode* node);
void _Dump(const AABBQuantizedNoLeafNode* node);
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Initializes a query
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
override(Collider) inline_ void InitQuery()
{
// Reset stats & contact status
mNbVolumeBVTests = 0;
mNbVolumePrimTests = 0;
Collider::InitQuery();
}
inline_ BOOL IsCacheValid(VolumeCache& cache)
{
// We're going to do a volume-vs-model query.
if(cache.Model!=mCurrentModel)
{
// Cached list was for another model so we can't keep it
// Keep track of new owner and reset cache
cache.Model = mCurrentModel;
return FALSE;
}
else
{
// Same models, no problem
return TRUE;
}
}
};
#endif // __OPC_VOLUMECOLLIDER_H__

65
ode/OPCODE/Opcode.cpp Normal file
View File

@ -0,0 +1,65 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Main file for Opcode.dll.
* \file Opcode.cpp
* \author Pierre Terdiman
* \date March, 20, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
Finding a good name is difficult!
Here's the draft for this lib.... Spooky, uh?
VOID? Very Optimized Interference Detection
ZOID? Zappy's Optimized Interference Detection
CID? Custom/Clever Interference Detection
AID / ACID! Accurate Interference Detection
QUID? Quick Interference Detection
RIDE? Realtime Interference DEtection
WIDE? Wicked Interference DEtection (....)
GUID!
KID ! k-dop interference detection :)
OPCODE! OPtimized COllision DEtection
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Precompiled Header
#include "Stdafx.h"
bool Opcode::InitOpcode()
{
Log("// Initializing OPCODE\n\n");
// LogAPIInfo();
return true;
}
void ReleasePruningSorters();
bool Opcode::CloseOpcode()
{
Log("// Closing OPCODE\n\n");
ReleasePruningSorters();
return true;
}
#ifdef ICE_MAIN
void ModuleAttach(HINSTANCE hinstance)
{
}
void ModuleDetach()
{
}
#endif

470
ode/OPCODE/Opcode.dsp Normal file
View File

@ -0,0 +1,470 @@
# Microsoft Developer Studio Project File - Name="OPCODE" - Package Owner=<4>
# Microsoft Developer Studio Generated Build File, Format Version 6.00
# ** DO NOT EDIT **
# TARGTYPE "Win32 (x86) Static Library" 0x0104
CFG=OPCODE - Win32 Debug
!MESSAGE This is not a valid makefile. To build this project using NMAKE,
!MESSAGE use the Export Makefile command and run
!MESSAGE
!MESSAGE NMAKE /f "Opcode.mak".
!MESSAGE
!MESSAGE You can specify a configuration when running NMAKE
!MESSAGE by defining the macro CFG on the command line. For example:
!MESSAGE
!MESSAGE NMAKE /f "Opcode.mak" CFG="OPCODE - Win32 Debug"
!MESSAGE
!MESSAGE Possible choices for configuration are:
!MESSAGE
!MESSAGE "OPCODE - Win32 Release" (based on "Win32 (x86) Static Library")
!MESSAGE "OPCODE - Win32 Debug" (based on "Win32 (x86) Static Library")
!MESSAGE
# Begin Project
# PROP AllowPerConfigDependencies 0
# PROP Scc_ProjName ""$/TR4/ODE/VC6", WNKAAAAA"
# PROP Scc_LocalPath "..\vc6"
CPP=cl.exe
RSC=rc.exe
!IF "$(CFG)" == "OPCODE - Win32 Release"
# PROP BASE Use_MFC 0
# PROP BASE Use_Debug_Libraries 0
# PROP BASE Output_Dir "Release"
# PROP BASE Intermediate_Dir "Release"
# PROP BASE Target_Dir ""
# PROP Use_MFC 0
# PROP Use_Debug_Libraries 0
# PROP Output_Dir "Release"
# PROP Intermediate_Dir "Release"
# PROP Target_Dir ""
# ADD BASE CPP /nologo /W3 /GX /O2 /D "WIN32" /D "NDEBUG" /D "_MBCS" /D "_LIB" /YX /FD /c
# ADD CPP /nologo /G6 /Zp4 /MD /O2 /Ob0 /I ".\\" /D "WIN32" /D "NDEBUG" /D "_MBCS" /D "_LIB" /D "ICE_NO_DLL" /FD /c
# SUBTRACT CPP /Fr /YX
# ADD BASE RSC /l 0x409 /d "NDEBUG"
# ADD RSC /l 0x409 /d "NDEBUG"
BSC32=bscmake.exe
# ADD BASE BSC32 /nologo
# ADD BSC32 /nologo
LIB32=link.exe -lib
# ADD BASE LIB32 /nologo
# ADD LIB32 /nologo /out:"..\lib\OPCODE.lib"
!ELSEIF "$(CFG)" == "OPCODE - Win32 Debug"
# PROP BASE Use_MFC 0
# PROP BASE Use_Debug_Libraries 1
# PROP BASE Output_Dir "Debug"
# PROP BASE Intermediate_Dir "Debug"
# PROP BASE Target_Dir ""
# PROP Use_MFC 0
# PROP Use_Debug_Libraries 1
# PROP Output_Dir "Debug"
# PROP Intermediate_Dir "Debug"
# PROP Target_Dir ""
# ADD BASE CPP /nologo /W3 /Gm /GX /ZI /Od /D "WIN32" /D "_DEBUG" /D "_MBCS" /D "_LIB" /YX /FD /GZ /c
# ADD CPP /nologo /G6 /Zp4 /MDd /Gm /ZI /Od /I ".\\" /D "WIN32" /D "_DEBUG" /D "_MBCS" /D "_LIB" /D "ICE_NO_DLL" /FR /FD /GZ /c
# SUBTRACT CPP /YX
# ADD BASE RSC /l 0x409 /d "_DEBUG"
# ADD RSC /l 0x409 /d "_DEBUG"
BSC32=bscmake.exe
# ADD BASE BSC32 /nologo
# ADD BSC32 /nologo
LIB32=link.exe -lib
# ADD BASE LIB32 /nologo
# ADD LIB32 /nologo /out:"..\lib\OPCODE_D.lib"
!ENDIF
# Begin Target
# Name "OPCODE - Win32 Release"
# Name "OPCODE - Win32 Debug"
# Begin Source File
SOURCE=.\Ice\IceAABB.cpp
# End Source File
# Begin Source File
SOURCE=.\Ice\IceAABB.h
# End Source File
# Begin Source File
SOURCE=.\Ice\IceAxes.h
# End Source File
# Begin Source File
SOURCE=.\Ice\IceBoundingSphere.h
# End Source File
# Begin Source File
SOURCE=.\Ice\IceContainer.cpp
# End Source File
# Begin Source File
SOURCE=.\Ice\IceContainer.h
# End Source File
# Begin Source File
SOURCE=.\Ice\IceFPU.h
# End Source File
# Begin Source File
SOURCE=.\Ice\IceHPoint.cpp
# End Source File
# Begin Source File
SOURCE=.\Ice\IceHPoint.h
# End Source File
# Begin Source File
SOURCE=.\Ice\IceIndexedTriangle.cpp
# End Source File
# Begin Source File
SOURCE=.\Ice\IceIndexedTriangle.h
# End Source File
# Begin Source File
SOURCE=.\Ice\IceLSS.h
# End Source File
# Begin Source File
SOURCE=.\Ice\IceMatrix3x3.cpp
# End Source File
# Begin Source File
SOURCE=.\Ice\IceMatrix3x3.h
# End Source File
# Begin Source File
SOURCE=.\Ice\IceMatrix4x4.cpp
# End Source File
# Begin Source File
SOURCE=.\Ice\IceMatrix4x4.h
# End Source File
# Begin Source File
SOURCE=.\Ice\IceMemoryMacros.h
# End Source File
# Begin Source File
SOURCE=.\Ice\IceOBB.cpp
# End Source File
# Begin Source File
SOURCE=.\Ice\IceOBB.h
# End Source File
# Begin Source File
SOURCE=.\Ice\IcePairs.h
# End Source File
# Begin Source File
SOURCE=.\Ice\IcePlane.cpp
# End Source File
# Begin Source File
SOURCE=.\Ice\IcePlane.h
# End Source File
# Begin Source File
SOURCE=.\Ice\IcePoint.cpp
# End Source File
# Begin Source File
SOURCE=.\Ice\IcePoint.h
# End Source File
# Begin Source File
SOURCE=.\Ice\IcePreprocessor.h
# End Source File
# Begin Source File
SOURCE=.\Ice\IceRandom.cpp
# End Source File
# Begin Source File
SOURCE=.\Ice\IceRandom.h
# End Source File
# Begin Source File
SOURCE=.\Ice\IceRay.cpp
# End Source File
# Begin Source File
SOURCE=.\Ice\IceRay.h
# End Source File
# Begin Source File
SOURCE=.\Ice\IceRevisitedRadix.cpp
# End Source File
# Begin Source File
SOURCE=.\Ice\IceRevisitedRadix.h
# End Source File
# Begin Source File
SOURCE=.\Ice\IceSegment.cpp
# End Source File
# Begin Source File
SOURCE=.\Ice\IceSegment.h
# End Source File
# Begin Source File
SOURCE=.\Ice\IceTriangle.cpp
# End Source File
# Begin Source File
SOURCE=.\Ice\IceTriangle.h
# End Source File
# Begin Source File
SOURCE=.\Ice\IceTrilist.h
# End Source File
# Begin Source File
SOURCE=.\Ice\IceTypes.h
# End Source File
# Begin Source File
SOURCE=.\Ice\IceUtils.cpp
# End Source File
# Begin Source File
SOURCE=.\Ice\IceUtils.h
# End Source File
# Begin Source File
SOURCE=.\OPC_AABBCollider.cpp
# End Source File
# Begin Source File
SOURCE=.\OPC_AABBCollider.h
# End Source File
# Begin Source File
SOURCE=.\OPC_AABBTree.cpp
# End Source File
# Begin Source File
SOURCE=.\OPC_AABBTree.h
# End Source File
# Begin Source File
SOURCE=.\OPC_BaseModel.cpp
# End Source File
# Begin Source File
SOURCE=.\OPC_BaseModel.h
# End Source File
# Begin Source File
SOURCE=.\OPC_BoxBoxOverlap.h
# End Source File
# Begin Source File
SOURCE=.\OPC_BoxPruning.cpp
# End Source File
# Begin Source File
SOURCE=.\OPC_BoxPruning.h
# End Source File
# Begin Source File
SOURCE=.\OPC_Collider.cpp
# End Source File
# Begin Source File
SOURCE=.\OPC_Collider.h
# End Source File
# Begin Source File
SOURCE=.\OPC_Common.cpp
# End Source File
# Begin Source File
SOURCE=.\OPC_Common.h
# End Source File
# Begin Source File
SOURCE=.\OPC_HybridModel.cpp
# End Source File
# Begin Source File
SOURCE=.\OPC_HybridModel.h
# End Source File
# Begin Source File
SOURCE=.\OPC_IceHook.h
# End Source File
# Begin Source File
SOURCE=.\OPC_LSSAABBOverlap.h
# End Source File
# Begin Source File
SOURCE=.\OPC_LSSCollider.cpp
# End Source File
# Begin Source File
SOURCE=.\OPC_LSSCollider.h
# End Source File
# Begin Source File
SOURCE=.\OPC_LSSTriOverlap.h
# End Source File
# Begin Source File
SOURCE=.\OPC_MeshInterface.cpp
# End Source File
# Begin Source File
SOURCE=.\OPC_MeshInterface.h
# End Source File
# Begin Source File
SOURCE=.\OPC_Model.cpp
# End Source File
# Begin Source File
SOURCE=.\OPC_Model.h
# End Source File
# Begin Source File
SOURCE=.\OPC_OBBCollider.cpp
# End Source File
# Begin Source File
SOURCE=.\OPC_OBBCollider.h
# End Source File
# Begin Source File
SOURCE=.\OPC_OptimizedTree.cpp
# End Source File
# Begin Source File
SOURCE=.\OPC_OptimizedTree.h
# End Source File
# Begin Source File
SOURCE=.\OPC_Picking.cpp
# End Source File
# Begin Source File
SOURCE=.\OPC_Picking.h
# End Source File
# Begin Source File
SOURCE=.\OPC_PlanesAABBOverlap.h
# End Source File
# Begin Source File
SOURCE=.\OPC_PlanesCollider.cpp
# End Source File
# Begin Source File
SOURCE=.\OPC_PlanesCollider.h
# End Source File
# Begin Source File
SOURCE=.\OPC_PlanesTriOverlap.h
# End Source File
# Begin Source File
SOURCE=.\OPC_RayAABBOverlap.h
# End Source File
# Begin Source File
SOURCE=.\OPC_RayCollider.cpp
# End Source File
# Begin Source File
SOURCE=.\OPC_RayCollider.h
# End Source File
# Begin Source File
SOURCE=.\OPC_RayTriOverlap.h
# End Source File
# Begin Source File
SOURCE=.\OPC_Settings.h
# End Source File
# Begin Source File
SOURCE=.\OPC_SphereAABBOverlap.h
# End Source File
# Begin Source File
SOURCE=.\OPC_SphereCollider.cpp
# End Source File
# Begin Source File
SOURCE=.\OPC_SphereCollider.h
# End Source File
# Begin Source File
SOURCE=.\OPC_SphereTriOverlap.h
# End Source File
# Begin Source File
SOURCE=.\OPC_SweepAndPrune.cpp
# End Source File
# Begin Source File
SOURCE=.\OPC_SweepAndPrune.h
# End Source File
# Begin Source File
SOURCE=.\OPC_TreeBuilders.cpp
# End Source File
# Begin Source File
SOURCE=.\OPC_TreeBuilders.h
# End Source File
# Begin Source File
SOURCE=.\OPC_TreeCollider.cpp
# End Source File
# Begin Source File
SOURCE=.\OPC_TreeCollider.h
# End Source File
# Begin Source File
SOURCE=.\OPC_TriBoxOverlap.h
# End Source File
# Begin Source File
SOURCE=.\OPC_TriTriOverlap.h
# End Source File
# Begin Source File
SOURCE=.\OPC_VolumeCollider.cpp
# End Source File
# Begin Source File
SOURCE=.\OPC_VolumeCollider.h
# End Source File
# Begin Source File
SOURCE=.\Opcode.cpp
# End Source File
# Begin Source File
SOURCE=.\Opcode.h
# End Source File
# Begin Source File
SOURCE=.\StdAfx.cpp
# End Source File
# Begin Source File
SOURCE=.\StdAfx.h
# End Source File
# End Target
# End Project

29
ode/OPCODE/Opcode.dsw Normal file
View File

@ -0,0 +1,29 @@
Microsoft Developer Studio Workspace File, Format Version 6.00
# WARNING: DO NOT EDIT OR DELETE THIS WORKSPACE FILE!
###############################################################################
Project: "OPCODE"=.\Opcode.dsp - Package Owner=<4>
Package=<5>
{{{
}}}
Package=<4>
{{{
}}}
###############################################################################
Global:
Package=<5>
{{{
}}}
Package=<3>
{{{
}}}
###############################################################################

113
ode/OPCODE/Opcode.h Normal file
View File

@ -0,0 +1,113 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Main file for Opcode.dll.
* \file Opcode.h
* \author Pierre Terdiman
* \date March, 20, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Include Guard
#ifndef __OPCODE_H__
#define __OPCODE_H__
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Things to help us compile on non-windows platforms
#if defined(__APPLE__) || defined(__MACOSX__)
#if __APPLE_CC__ < 1495
#define sqrtf sqrt
#define sinf sin
#define cosf cos
#define acosf acos
#define asinf asin
#endif
#endif
#ifndef _MSC_VER
#ifndef __int64
#define __int64 long long int
#endif
#ifndef __stdcall /* this is defined in MinGW and CygWin, so avoid the warning */
#define __stdcall /* */
#endif
#endif
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Compilation messages
#ifdef _MSC_VER
#if defined(OPCODE_EXPORTS)
// #pragma message("Compiling OPCODE")
#elif !defined(OPCODE_EXPORTS)
// #pragma message("Using OPCODE")
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Automatic linking
#ifndef BAN_OPCODE_AUTOLINK
#ifdef _DEBUG
//#pragma comment(lib, "Opcode_D.lib")
#else
//#pragma comment(lib, "Opcode.lib")
#endif
#endif
#endif
#endif
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Preprocessor
#ifndef ICE_NO_DLL
#ifdef OPCODE_EXPORTS
#define OPCODE_API// __declspec(dllexport)
#else
#define OPCODE_API// __declspec(dllimport)
#endif
#else
#define OPCODE_API
#endif
#include "OPC_IceHook.h"
namespace Opcode
{
// Bulk-of-the-work
#include "OPC_Settings.h"
#include "OPC_Common.h"
#include "OPC_MeshInterface.h"
// Builders
#include "OPC_TreeBuilders.h"
// Trees
#include "OPC_AABBTree.h"
#include "OPC_OptimizedTree.h"
// Models
#include "OPC_BaseModel.h"
#include "OPC_Model.h"
#include "OPC_HybridModel.h"
// Colliders
#include "OPC_Collider.h"
#include "OPC_VolumeCollider.h"
#include "OPC_TreeCollider.h"
#include "OPC_RayCollider.h"
#include "OPC_SphereCollider.h"
#include "OPC_OBBCollider.h"
#include "OPC_AABBCollider.h"
#include "OPC_LSSCollider.h"
#include "OPC_PlanesCollider.h"
// Usages
#include "OPC_Picking.h"
// Sweep-and-prune
#include "OPC_BoxPruning.h"
#include "OPC_SweepAndPrune.h"
FUNCTION OPCODE_API bool InitOpcode();
FUNCTION OPCODE_API bool CloseOpcode();
}
#endif // __OPCODE_H__

17
ode/OPCODE/README-ODE.txt Normal file
View File

@ -0,0 +1,17 @@
This is a copy of the OPCODE collision detection library by Pierre Terdiman.
See http://www.codercorner.com/Opcode.htm for more information, and read
the ReadMe.txt in this directory.
If you want to use the TriList (triangle mesh) geometry class in ODE, the
OPCODE library must be compiled. To do so, simply uncomment the
OPCODE_DIRECTORY variable in ODE's user-settings file, and ODE's default build
system will compile OPCODE for you.
If you are using the experimental autotools support to compile ODE, you just
have to specify --with-opcode when calling ./configure.
This code was originally written for and compiled on windows, but it has been
ported so that it should compile under unix/gcc too. Your mileage may vary.
Russ Smith, April 12 2005.

171
ode/OPCODE/ReadMe.txt Normal file
View File

@ -0,0 +1,171 @@
OPCODE distribution 1.3 (june 2003)
-----------------------
New in Opcode 1.3:
- fixed the divide by 0 bug that was happening when all centers where located on a coordinate axis (thanks to Jorrit T)
- linearized "complete" vanilla AABB trees
- ANSI-compliant "for" loops (for the ones porting it to Linux...)
- callbacks & pointers moved to mesh interface
- support for triangle & vertex strides
- optimized the sphere-triangle overlap code a bit
- dynamic trees (refit)
- more builders
- ValidateSubdivision in builders
- LSS collider
- primitive-bv tests can now be skipped in most volume queries
- temporal coherence now also works for airborne objects
- temporal coherence completed for boxes / all contacts, LSS, etc
- ray-collider now uses a callback
- some common "usages" have been introduced (only picking for now)
- SPLIT_COMPLETE removed (now implicitely using mLimit = 1)
- hybrid collision models
- sweep-and-prune code added, moved from my old Z-Collide lib
- it now works with meshes made of only 1 triangle (except in mesh-mesh case!)
Disclaimer:
- I forced myself to actually *do* the release today no matter what. Else it would never have been done. That's
why the code may not be very polished. I also removed a *lot* of things (more usages, distance queries, etc...)
that weren't ready for prime-time (or that were linked to too many of my supporting libs)
- Some comments may also be obsolete here and there. The old User Manual for Opcode 1.2 may not fit version 1.3
either, since there's a new "mesh interface" to support strides, etc.
- Everything in the "Ice" directory has been hacked out of my engine and edited until everything compiled. Don't
expect anything out there to be cute or something. In particular, some CPP files are not even included when not
needed, so you can expect some linker errors if you try messing around with them...
Otherwise, it should be just like previous version, only better. In particular, hybrid models can be very
memory-friendly (sometimes using like 10 times less ram than the best trees from version 1.2). The possible
speed hit is often invisible (if it even exists), especially using temporal coherence in "all contacts" mode.
(Admittedly, this depends on your particular usage pattern / what you do on collided triangles).
The sweep-and-prune code is similar to the "vanilla" version found in V-Collide (but that one's better IMHO...)
The simple "radix" version is often just as good, see for yourself.
OPCODE distribution 1.2 (august 2002)
-----------------------
New in Opcode 1.2:
- new VolumeCollider base class
- simplified callback setup
- you can now use callbacks or pointers (setup at compile time)
- destination array not needed anymore in the RayCollider (faster in-out tests)
- renamed classes: AABBRayCollider => RayCollider, AABBSphereCollider => SphereCollider
- the sphere query now only returns a list of faces (extra info discarded). On the other hand it's a lot faster.
- OBB, AABB and planes queries. Original OBB and AABB queries contributed by Erwin de Vries.
- cosmetic changes in OPC_BoxBoxOverlap.h contributed by Gottfried Chen
- some inlining problems fixed
- faster ray-mesh tests using the separating axis theorem
- new split value in AABB tree construction (contributed by Igor Kravtchenko). Provides faster queries most of the time.
- improved temporal coherence for sphere & AABB queries (works in "All contacts" mode)
Notes:
- Everything in the "Ice code" directory (in VC++) is basically copy-pasted from my engine, with a lot
of code removed until there was no link error anymore. Don't expect those files to be cute or anything,
they've never been meant to be released and they're often updated/modified/messy.
- Some experimental features have been removed as well. Else I would never have released the 1.2...
- Not as polished/optimal as I would like it to be, but that's life. I promised myself to release it
before october 2002 (one YEAR later ?!).... That's the only reason why it's there.
- Some people reported ColDet was faster. Uh, come on. They were using Opcode in
"All contacts" mode whereas ColDet was doing "first contact"...
OPCODE distribution 1.1 (october 2001)
-----------------------
New in Opcode 1.1:
- stabbing queries
- sphere queries
- abtract base class for colliders
- settings validation methods
- compilation flags now grouped in OPC_Settings.h
- smaller files, new VC++ virtual dirs (cleaner)
Notes:
- "override(baseclass)" is a personal cosmetic thing. It's the same as "virtual", but provides more info.
- I code in 1600*1200, so some lines may look a bit long..
- This version is not as polished as the previous one due to lack of time. The stabbing & sphere queries
can still be optimized: for example by trying other atomic overlap tests. I'm using my first ray-AABB
code, but the newer one seems better. Tim Schr<68>der's one is good as well. See: www.codercorner.com/RayAABB.cpp
- The trees can easily be compressed even more, I save this for later (lack of time, lack of time!)
- I removed various tests before releasing this one:
- a separation line, a.k.a. "front" in QuickCD, because gains were unclear
- distance queries in a PQP style, because it was way too slow
- support for deformable models, too slow as well
- You can easily use Opcode to do your player-vs-world collision detection, in a Nettle/Telemachos way.
If someone out there wants to donate some art / level for the cause, I'd be glad to release a demo. (current
demo uses copyrighted art I'm not allowed to spread)
- Sorry for the lack of real docs and/or solid examples. I just don't have enough time.
OPCODE distribution 1.0 (march 2001)
-----------------------
- First release
===============================================================================
WHAT ?
OPCODE means OPtimized COllision DEtection.
So this is a collision detection package similar to RAPID. Here's a
quick list of features:
- C++ interface, developed for Windows systems using VC++ 6.0
- Works on arbitrary meshes (convex or non-convex), even polygon soups
- Current implementation uses AABB-trees
- Introduces Primitive-BV overlap tests during recursive collision queries (whereas
standard libraries only rely on Primitive-Primitive and BV-BV tests)
- Introduces no-leaf trees, i.e. collision trees whose leaf nodes have been removed
- Supports collision queries on quantized trees (decompressed on-the-fly)
- Supports "first contact" or "all contacts" modes (<28> la RAPID)
- Uses temporal coherence for "first contact" mode (~10 to 20 times faster, useful
in rigid body simulation during bisection)
- Memory footprint is 7.2 times smaller than RAPID's one, which is ideal for console
games with limited ram (actually, if you use the unmodified RAPID code using double
precision, it's more like 13 times smaller...)
- And yet it often runs faster than RAPID (according to RDTSC, sometimes more than 5
times faster when objects are deeply overlapping)
- Performance is usually close to RAPID's one in close-proximity situations
- Stabbing, planes & volume queries (sphere, AABB, OBB, LSS)
- Sweep-and-prune
- Now works with deformable meshes
- Hybrid trees
What it can be used for:
- standard mesh-mesh collision detection (similar to RAPID, SOLID, QuickCD, PQP, ColDet...)
- N-body collisions (similar to V-Collide)
- camera-vs-world collisions (similar to Telemachos/Paul Nettle/Stan Melax articles)
- shadow feelers to speed up lightmap computations
- in-out tests to speed up voxelization processes
- picking
- rigid body simulation
- view frustum culling
- etc
WHY ?
- Because RAPID uses too many bytes.
- Because the idea was nice...
WHEN ?
It's been coded in march 2001 following a thread on the GD-Algorithms list.
GDAlgorithms-list mailing list
GDAlgorithms-list@lists.sourceforge.net
http://lists.sourceforge.net/lists/listinfo/gdalgorithms-list
WHO ?
Pierre Terdiman
June, 1, 2003
p.terdiman@wanadoo.fr
p.terdiman@codercorner.com
http://www.codercorner.com
http://www.codercorner.com/Opcode.htm

10
ode/OPCODE/StdAfx.cpp Normal file
View File

@ -0,0 +1,10 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//#define ICE_MAIN
#include "Stdafx.h"

24
ode/OPCODE/Stdafx.h Normal file
View File

@ -0,0 +1,24 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
#if !defined(AFX_STDAFX_H__EFB95044_1D31_11D5_8B0F_0050BAC83302__INCLUDED_)
#define AFX_STDAFX_H__EFB95044_1D31_11D5_8B0F_0050BAC83302__INCLUDED_
#if _MSC_VER > 1000
#pragma once
#endif // _MSC_VER > 1000
// Insert your headers here
#define WIN32_LEAN_AND_MEAN // Exclude rarely-used stuff from Windows headers
#include "Opcode.h"
//{{AFX_INSERT_LOCATION}}
// Microsoft Visual C++ will insert additional declarations immediately before the previous line.
#endif // !defined(AFX_STDAFX_H__EFB95044_1D31_11D5_8B0F_0050BAC83302__INCLUDED_)

Some files were not shown because too many files have changed in this diff Show More