508 lines
13 KiB
C++
508 lines
13 KiB
C++
/*
|
|
Copyright (c) 2012 Advanced Micro Devices, Inc.
|
|
|
|
This software is provided 'as-is', without any express or implied warranty.
|
|
In no event will the authors be held liable for any damages arising from the use of this software.
|
|
Permission is granted to anyone to use this software for any purpose,
|
|
including commercial applications, and to alter it and redistribute it freely,
|
|
subject to the following restrictions:
|
|
|
|
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
|
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
|
3. This notice may not be removed or altered from any source distribution.
|
|
*/
|
|
//Originally written by Takahiro Harada
|
|
|
|
|
|
#include "ConvexHeightFieldShape.h"
|
|
#include "Stubs/AdlCollideUtils.h"
|
|
#include "CubeMapUtils.h"
|
|
//#include <common/Physics/ShapeBase.h>
|
|
//#include <common/Physics/SphereShape.h>
|
|
//#include "GlutStuff.h"
|
|
|
|
//#define USE_OLD
|
|
|
|
ConvexHeightField::ConvexHeightField(const float4* vtxBuffer, const int4* idxBuffer, int nTriangles)
|
|
: CollisionShape( SHAPE_CONVEX_HEIGHT_FIELD )
|
|
{
|
|
create( vtxBuffer, idxBuffer, nTriangles );
|
|
}
|
|
|
|
void ConvexHeightField::create( const float4* vtxBuffer, const int4* idxBuffer, int nTriangles )
|
|
{
|
|
{
|
|
float maxDx2 = -1.f;
|
|
int maxIdx = -1;
|
|
for(int i=0; i<nTriangles; i++)
|
|
{
|
|
const int4& idx = idxBuffer[i];
|
|
for(int j=0; j<3; j++)
|
|
{
|
|
float dx2 = dot3F4( vtxBuffer[idx.s[j]], vtxBuffer[idx.s[j]] );
|
|
if( dx2 > maxDx2 )
|
|
{
|
|
maxDx2 = dx2;
|
|
maxIdx = idx.s[j];
|
|
}
|
|
}
|
|
}
|
|
ADLASSERT( maxIdx != -1 );
|
|
m_scale = sqrtf( maxDx2 );
|
|
}
|
|
|
|
// cast ray to find intersectPlaneLineions
|
|
{
|
|
for(u32 faceIdx=0; faceIdx<6; faceIdx++)
|
|
{
|
|
for(int i=0; i<HEIGHT_RES; i++) for(int j=0; j<HEIGHT_RES; j++)
|
|
{
|
|
float4 v;
|
|
float x = (i+0.5f)/(float)HEIGHT_RES;
|
|
float y = (j+0.5f)/(float)HEIGHT_RES;
|
|
v = CubeMapUtils::calcVector(faceIdx, x, y);
|
|
v = normalize3( v );
|
|
v *= m_scale;
|
|
|
|
float minFraction = FLT_MAX;
|
|
float4 minNormal;
|
|
float4 minBCrd;
|
|
for(int itri=0; itri<nTriangles; itri++)
|
|
{
|
|
float4 from = make_float4(0.f);
|
|
float4 bCrd;
|
|
float fraction = CollideUtils::castRay( vtxBuffer[idxBuffer[itri].x], vtxBuffer[idxBuffer[itri].y], vtxBuffer[idxBuffer[itri].z],
|
|
from, v, 0.0f, &bCrd );
|
|
|
|
if( fraction > 0.f )
|
|
{
|
|
minFraction = min2( minFraction, fraction ); // todo. have to check if this is the min to replace normal?
|
|
float4 ab = vtxBuffer[idxBuffer[itri].y]-vtxBuffer[idxBuffer[itri].x];
|
|
float4 ac = vtxBuffer[idxBuffer[itri].z]-vtxBuffer[idxBuffer[itri].x];
|
|
minNormal = cross3( ab, ac );
|
|
minBCrd = bCrd;
|
|
}
|
|
}
|
|
|
|
if( minFraction == FLT_MAX )
|
|
minFraction = 0.f;
|
|
|
|
{
|
|
u8 quantizedHeight = (u8)(minFraction*255.f);
|
|
sample( (Face)faceIdx, i,j ) = quantizedHeight;
|
|
sampleNormal( (Face)faceIdx, i,j ) = normalize3(minNormal);
|
|
float minValue = 3.f*(1.f/3.f)*(1.f/3.f);
|
|
sampleNormal( (Face)faceIdx, i,j ).w = (dot3F4( minBCrd, minBCrd ) - minValue )/(1.f-minValue);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
calcSamplePoints( m_samplePoints );
|
|
|
|
// calc support height using m_samplePoints
|
|
{
|
|
for(u32 faceIdx=0; faceIdx<6; faceIdx++) for(int i=0; i<HEIGHT_RES; i++) for(int j=0; j<HEIGHT_RES; j++)
|
|
{
|
|
float4 v;
|
|
float x = (i+0.5f)/(float)HEIGHT_RES;
|
|
float y = (j+0.5f)/(float)HEIGHT_RES;
|
|
v = CubeMapUtils::calcVector(faceIdx, x, y);
|
|
v = normalize3( v );
|
|
|
|
float maxHeight = -1;
|
|
for(int ie=0; ie<6*HEIGHT_RES*HEIGHT_RES; ie++)
|
|
{
|
|
float h = dot3F4( v, m_samplePoints[ie] )/m_scale;
|
|
ADLASSERT( h <= 1.f );
|
|
if( h > maxHeight ) maxHeight = h;
|
|
}
|
|
|
|
{
|
|
u8 quantizedHeight = min2((u8)(maxHeight*255.f)+1, 255);
|
|
sampleSupport( (Face)faceIdx, i, j ) = quantizedHeight;
|
|
}
|
|
}
|
|
}
|
|
|
|
m_aabb.setEmpty();
|
|
for(int i=0; i<nTriangles; i++)
|
|
{
|
|
const int4& idx = idxBuffer[i];
|
|
m_aabb.includePoint( vtxBuffer[idx.x] );
|
|
m_aabb.includePoint( vtxBuffer[idx.y] );
|
|
m_aabb.includePoint( vtxBuffer[idx.z] );
|
|
}
|
|
m_aabb.expandBy( make_float4( m_collisionMargin ) );
|
|
|
|
for(int i=0; i<6; i++)
|
|
{
|
|
m_faceAabbs[i].setEmpty();
|
|
for(int j=0; j<HEIGHT_RES*HEIGHT_RES; j++)
|
|
{
|
|
float4 p = m_samplePoints[i*HEIGHT_RES*HEIGHT_RES + j];
|
|
m_faceAabbs[i].includePoint(p);
|
|
}
|
|
m_faceAabbs[i].expandBy( make_float4( m_collisionMargin ) );
|
|
}
|
|
}
|
|
|
|
static __inline float localIntersectPlaneLine( const float4& planeEqn, const float4& vec, const float4& orig )
|
|
{
|
|
return (-planeEqn.w - dot3F4(planeEqn, orig))/dot3F4(planeEqn, vec);
|
|
}
|
|
|
|
|
|
ConvexHeightField::ConvexHeightField(const float4* eqn, int nEqn)
|
|
: CollisionShape( SHAPE_CONVEX_HEIGHT_FIELD )
|
|
{
|
|
{ // cast ray to find intersectPlaneLineions
|
|
for(u32 faceIdx=0; faceIdx<6; faceIdx++)
|
|
{
|
|
for(int i=0; i<HEIGHT_RES; i++) for(int j=0; j<HEIGHT_RES; j++)
|
|
{
|
|
float4 v;
|
|
float x = (i+0.5f)/(float)HEIGHT_RES;
|
|
float y = (j+0.5f)/(float)HEIGHT_RES;
|
|
v = CubeMapUtils::calcVector(faceIdx, x, y);
|
|
v = normalize3( v );
|
|
|
|
float minFraction = FLT_MAX;
|
|
float4 minNormal;
|
|
for(int ii=0; ii<nEqn; ii++)
|
|
{
|
|
const float4& iEqn = eqn[ii];
|
|
|
|
float fraction = localIntersectPlaneLine( iEqn, v, make_float4(0.f) );
|
|
|
|
if( fraction > 0.f )
|
|
{
|
|
if( fraction < minFraction )
|
|
{
|
|
minFraction = fraction;
|
|
minNormal = iEqn;
|
|
}
|
|
}
|
|
}
|
|
|
|
ADLASSERT( minFraction != FLT_MAX );
|
|
|
|
minNormal.w = minFraction;
|
|
sampleNormal( (Face)faceIdx, i, j ) = minNormal;
|
|
}
|
|
}
|
|
}
|
|
|
|
{
|
|
m_scale = -FLT_MAX;
|
|
for(u32 faceIdx=0; faceIdx<6; faceIdx++)
|
|
{
|
|
for(int i=0; i<HEIGHT_RES; i++) for(int j=0; j<HEIGHT_RES; j++)
|
|
{
|
|
float4& n = sampleNormal( (Face)faceIdx, i, j );
|
|
|
|
m_scale = max2( m_scale, n.w );
|
|
}
|
|
}
|
|
|
|
for(u32 faceIdx=0; faceIdx<6; faceIdx++)
|
|
{
|
|
for(int i=0; i<HEIGHT_RES; i++) for(int j=0; j<HEIGHT_RES; j++)
|
|
{
|
|
float4& n = sampleNormal( (Face)faceIdx, i, j );
|
|
u8 quantizedHeight = (u8)(n.w/m_scale*255.f);
|
|
sample( (Face)faceIdx, i, j ) = quantizedHeight;
|
|
}
|
|
}
|
|
}
|
|
|
|
calcSamplePoints( m_samplePoints );
|
|
|
|
// calc support height using m_samplePoints
|
|
{
|
|
for(u32 faceIdx=0; faceIdx<6; faceIdx++) for(int i=0; i<HEIGHT_RES; i++) for(int j=0; j<HEIGHT_RES; j++)
|
|
{
|
|
float4 v;
|
|
float x = (i+0.5f)/(float)HEIGHT_RES;
|
|
float y = (j+0.5f)/(float)HEIGHT_RES;
|
|
v = CubeMapUtils::calcVector(faceIdx, x, y);
|
|
v = normalize3( v );
|
|
|
|
float maxHeight = -1;
|
|
for(int ie=0; ie<6*HEIGHT_RES*HEIGHT_RES; ie++)
|
|
{
|
|
float h = dot3F4( v, m_samplePoints[ie] )/m_scale;
|
|
if (h>1.f)
|
|
h=1.f;
|
|
// ADLASSERT( h <= 1.f );
|
|
if( h > maxHeight ) maxHeight = h;
|
|
}
|
|
|
|
{
|
|
u8 quantizedHeight = min2((u8)(maxHeight*255.f)+1, 255);
|
|
sampleSupport( (Face)faceIdx, i, j ) = quantizedHeight;
|
|
}
|
|
}
|
|
}
|
|
|
|
for(int i=0; i<6; i++)
|
|
{
|
|
m_faceAabbs[i].setEmpty();
|
|
for(int j=0; j<HEIGHT_RES*HEIGHT_RES; j++)
|
|
{
|
|
float4 p = m_samplePoints[i*HEIGHT_RES*HEIGHT_RES + j];
|
|
m_faceAabbs[i].includePoint(p);
|
|
}
|
|
m_faceAabbs[i].expandBy( make_float4( m_collisionMargin ) );
|
|
}
|
|
|
|
m_aabb.setEmpty();
|
|
for(int i=0; i<6; i++)
|
|
{
|
|
m_aabb.includeVolume( m_faceAabbs[i] );
|
|
}
|
|
}
|
|
|
|
#if 0
|
|
ConvexHeightField::ConvexHeightField(const ShapeBase* shape)
|
|
: CollisionShape( SHAPE_CONVEX_HEIGHT_FIELD )
|
|
{
|
|
if( shape->m_type == ADL_SHAPE_SPHERE )
|
|
{
|
|
SphereShape* sphere = (SphereShape*)shape;
|
|
|
|
m_scale = sphere->m_radius;
|
|
for(u32 faceIdx=0; faceIdx<6; faceIdx++)
|
|
{
|
|
for(int i=0; i<HEIGHT_RES; i++) for(int j=0; j<HEIGHT_RES; j++)
|
|
{
|
|
float4 minNormal;
|
|
float x = (i+0.5f)/(float)HEIGHT_RES;
|
|
float y = (j+0.5f)/(float)HEIGHT_RES;
|
|
minNormal = CubeMapUtils::calcVector(faceIdx, x, y);
|
|
minNormal = normalize3( minNormal );
|
|
{
|
|
u8 quantizedHeight = (u8)(1.f*255.f);
|
|
sample( (Face)faceIdx, i,j ) = quantizedHeight;
|
|
sampleNormal( (Face)faceIdx, i,j ) = normalize3(minNormal);
|
|
// float minValue = 3.f*(1.f/3.f)*(1.f/3.f);
|
|
// sampleNormal( (Face)faceIdx, i,j ).w = (dot3F4( minBCrd, minBCrd ) - minValue )/(1.f-minValue);
|
|
}
|
|
}
|
|
}
|
|
|
|
calcSamplePoints( m_samplePoints );
|
|
|
|
m_aabb.m_max = make_float4( sphere->m_radius );
|
|
m_aabb.m_min = make_float4( -sphere->m_radius );
|
|
|
|
m_aabb.expandBy( make_float4( m_collisionMargin ) );
|
|
|
|
for(int i=0; i<6; i++)
|
|
{
|
|
m_faceAabbs[i].setEmpty();
|
|
for(int j=0; j<HEIGHT_RES*HEIGHT_RES; j++)
|
|
{
|
|
float4 p = m_samplePoints[i*HEIGHT_RES*HEIGHT_RES + j];
|
|
m_faceAabbs[i].includePoint(p);
|
|
}
|
|
m_faceAabbs[i].expandBy( make_float4( m_collisionMargin ) );
|
|
}
|
|
}
|
|
else
|
|
{
|
|
ShapeBase* s = (ShapeBase*)shape;
|
|
|
|
create( s->getVertexBuffer(), s->getTriangleBuffer(), s->getNumTris() );
|
|
}
|
|
}
|
|
#endif
|
|
|
|
ConvexHeightField::~ConvexHeightField()
|
|
{
|
|
|
|
}
|
|
|
|
float ConvexHeightField::queryDistance(const float4& p ) const
|
|
{
|
|
const float4 majorAxes[] = {make_float4(1,0,0,0), make_float4(0,1,0,0), make_float4(0,0,1,0)};
|
|
|
|
if( dot3F4( p, p ) >= m_scale*m_scale ) return FLT_MAX;
|
|
|
|
int faceIdx;
|
|
float x, y;
|
|
CubeMapUtils::calcCrd( p, faceIdx, x, y );
|
|
x = (x*HEIGHT_RES) - 0.5f;
|
|
y = (y*HEIGHT_RES) - 0.5f;
|
|
|
|
float height;
|
|
{
|
|
int xi = (int)(x);
|
|
int yi = (int)(y);
|
|
float dx = x-xi;
|
|
float dy = y-yi;
|
|
|
|
{
|
|
int xip = min2((int)(HEIGHT_RES-1), xi+1);
|
|
int yip = min2((int)(HEIGHT_RES-1), yi+1);
|
|
|
|
u8 xy = sample( (Face)faceIdx, xi, yi );
|
|
u8 xpy = sample( (Face)faceIdx, xip, yi );
|
|
u8 xpyp = sample( (Face)faceIdx, xip, yip );
|
|
u8 xyp = sample( (Face)faceIdx, xi, yip );
|
|
|
|
height = (xy*(1.f-dx)+xpy*dx)*(1.f-dy) + (xyp*(1.f-dx)+xpyp*dx)*dy;
|
|
height = height/255.f*m_scale;
|
|
|
|
height = length3( p ) - height;
|
|
}
|
|
}
|
|
|
|
return height;
|
|
}
|
|
|
|
float ConvexHeightField::querySupportHeight(const float4& p ) const
|
|
{
|
|
const float4 majorAxes[] = {make_float4(1,0,0,0), make_float4(0,1,0,0), make_float4(0,0,1,0)};
|
|
|
|
// if( dot3F4( p, p ) >= m_scale*m_scale ) return FLT_MAX;
|
|
|
|
int faceIdx;
|
|
float x, y;
|
|
CubeMapUtils::calcCrd( p, faceIdx, x, y );
|
|
x = (x*HEIGHT_RES) - 0.5f;
|
|
y = (y*HEIGHT_RES) - 0.5f;
|
|
|
|
float height;
|
|
{
|
|
int xi = (int)(x);
|
|
int yi = (int)(y);
|
|
float dx = x-xi;
|
|
float dy = y-yi;
|
|
|
|
{
|
|
int xip = min2((int)(HEIGHT_RES-1), xi+1);
|
|
int yip = min2((int)(HEIGHT_RES-1), yi+1);
|
|
|
|
u8 xy = sampleSupport( (Face)faceIdx, xi, yi );
|
|
u8 xpy = sampleSupport( (Face)faceIdx, xip, yi );
|
|
u8 xpyp = sampleSupport( (Face)faceIdx, xip, yip );
|
|
u8 xyp = sampleSupport( (Face)faceIdx, xi, yip );
|
|
|
|
height = max2( xy, max2( xpy, max2( xpyp, xyp ) ) );
|
|
height = height/255.f*m_scale;
|
|
}
|
|
}
|
|
|
|
return height;
|
|
}
|
|
|
|
float ConvexHeightField::queryW(const float4& p ) const
|
|
{
|
|
const float4 majorAxes[] = {make_float4(1,0,0,0), make_float4(0,1,0,0), make_float4(0,0,1,0)};
|
|
|
|
float value;
|
|
if( dot3F4( p, p ) >= m_scale*m_scale ) return 0;
|
|
|
|
int faceIdx;
|
|
float x, y;
|
|
CubeMapUtils::calcCrd( p, faceIdx, x, y );
|
|
x = (x*HEIGHT_RES) - 0.5f;
|
|
y = (y*HEIGHT_RES) - 0.5f;
|
|
|
|
{
|
|
int xi = (int)(x);
|
|
int yi = (int)(y);
|
|
|
|
value = sampleNormal( (Face)faceIdx, xi, yi ).w;
|
|
}
|
|
return value;
|
|
}
|
|
|
|
bool ConvexHeightField::queryDistanceWithNormal( const float4& p, float4& normalOut ) const
|
|
{
|
|
int faceIdx;
|
|
float x, y;
|
|
CubeMapUtils::calcCrd( p, faceIdx, x, y );
|
|
x = (x*HEIGHT_RES) - 0.5f;
|
|
y = (y*HEIGHT_RES) - 0.5f;
|
|
|
|
{
|
|
int xi = (int)(x);
|
|
int yi = (int)(y);
|
|
|
|
normalOut = sampleNormal( (Face)faceIdx, xi, yi );
|
|
}
|
|
return true;
|
|
}
|
|
|
|
void ConvexHeightField::calcSamplePoints(float4* points) const
|
|
{
|
|
for(u32 faceIdx=0; faceIdx<6; faceIdx++)
|
|
{
|
|
for(int i=0; i<HEIGHT_RES; i++) for(int j=0; j<HEIGHT_RES; j++)
|
|
{
|
|
float4 v;
|
|
float x = (i+0.5f)/(float)HEIGHT_RES;
|
|
float y = (j+0.5f)/(float)HEIGHT_RES;
|
|
v = CubeMapUtils::calcVector(faceIdx, x, y);
|
|
v = normalize3( v );
|
|
|
|
int quantizedHeight = sample( (Face)faceIdx, i, j );
|
|
float rheight = quantizedHeight/255.f*m_scale;
|
|
|
|
points[ HEIGHT_RES*HEIGHT_RES*faceIdx + i + j*HEIGHT_RES ] = rheight*v;
|
|
}
|
|
}
|
|
return;
|
|
}
|
|
|
|
float4 ConvexHeightField::calcSamplePoint( int sIdx ) const
|
|
{
|
|
int idir; int plus;
|
|
Face faceIdx = (Face)(sIdx/(HEIGHT_RES*HEIGHT_RES));
|
|
idir = (faceIdx/2);
|
|
plus = faceIdx & 1;
|
|
|
|
float4 viewVector = make_float4((idir==0)?1.f:0.f, (idir==1)?1.f:0.f, (idir==2)?1.f:0.f );
|
|
if( plus==0 ) viewVector *= -1.f;
|
|
float4 xVector = make_float4( viewVector.z, viewVector.x, viewVector.y );
|
|
float4 yVector = make_float4( viewVector.y, viewVector.z, viewVector.x );
|
|
float4 orig = viewVector-xVector-yVector;
|
|
|
|
int pIdx = sIdx%(HEIGHT_RES*HEIGHT_RES);
|
|
int i = pIdx/HEIGHT_RES;
|
|
int j = pIdx%HEIGHT_RES;
|
|
|
|
float4 v = orig + (i+0.5f)*xVector/(HEIGHT_RES*0.5f) + (j+0.5f)*yVector/(HEIGHT_RES*0.5f);
|
|
v = normalize3( v );
|
|
|
|
int quantizedHeight = sample( faceIdx, i, j );
|
|
float rheight = quantizedHeight/255.f*m_scale;
|
|
return rheight*v;
|
|
}
|
|
|
|
const float4* ConvexHeightField::getSamplePoints() const
|
|
{
|
|
return m_samplePoints;
|
|
}
|
|
|
|
int ConvexHeightField::getNumSamplePoints() const
|
|
{
|
|
return HEIGHT_RES*HEIGHT_RES*6;
|
|
}
|
|
|
|
__inline
|
|
float4 rainbowMap( float s )
|
|
{
|
|
float c = 4.f;
|
|
float r,g,b;
|
|
r = c*(s-0.75f);
|
|
g = c*(s-0.5f);
|
|
b = c*(s-0.25f);
|
|
|
|
float4 col = make_float4( 1.f-r*r, 1.f-g*g, 1.f-b*b );
|
|
return col;
|
|
}
|
|
|