HyperPlane<dimension>::VectorsDriver::VectorsDriver(const VecD& origin, const VecD& normal, const RepereD* reference ) : Driver(), m_reference(reference), m_hasCov(false)
{
m_origin(dimension) = 1.0;
ublas::subrange(m_origin, 0, dimension ) = origin;
m_normal(dimension) = 0.0;
ublas::subrange(m_normal, 0, dimension ) = normal / ublas::norm_2(normal);
}
// warning: a work in progress...not tested yet
bool
CelAnimMesh::collideBox(int frameIndex,
const TMat3F & trans, // from box to mesh space (box center to origin)
const TMat3F & invTrans, // from mesh to box space
const Point3F &radii,
CollisionSurface & cs) const
{
cs;
int hitface = -1;
float hitDepth;
Point3F hitNormal;
float overlap;
AssertFatal( fFrames.size() > 0, "Shape must have at least one frame." );
AssertFatal( frameIndex >= 0 && frameIndex < fFrames.size(),
"TS::CelAnimMesh: frame index out of range" );
// get the frame struct:
const Frame *frm = &fFrames[frameIndex];
int fv = frm->fFirstVert;
const Point3F *pScale = &frm->fScale;
const Point3F *pOrigin = &frm->fOrigin;
int i;
Point3F v[3],tv[3]; // tv is work space for m_polyOBox...
for (i=0;i<fFaces.size();i++)
{
const Face & theFace = fFaces[i];
fVerts[theFace.fVIP[0].fVertexIndex+fv].getPoint(v[0],*pScale,*pOrigin);
fVerts[theFace.fVIP[1].fVertexIndex+fv].getPoint(v[1],*pScale,*pOrigin);
fVerts[theFace.fVIP[2].fVertexIndex+fv].getPoint(v[2],*pScale,*pOrigin);
// build the normal
Point3F normal;
m_normal(v[0],v[1],v[2],normal);
if (!m_polyOBox(radii,trans,invTrans,normal,v,tv,3,overlap))
continue;
/*
// build the normal
Point3F normal;
m_normal(v1,v2,v3,normal);
float planeDist = m_dot(normal,v1);
float negBoxDist = m_dot(normal,trans.p); // negative of dist of box from origin
if (planeDist+negBoxDist<0.0f) // back-face from box center
continue;
// does the face's plane intersect the box
float overlap;
if (!m_planeOBox(bRadii,trans,normal,planeDist,overlap))
continue;
Point3F tv1,tv2,tv3,tMin,tMax;
m_mul(v1,invTrans,&tv1);
m_mul(v2,invTrans,&tv2);
m_mul(v3,invTrans,&tv3);
tMin=tv1;
tMin.setMin(tv2);
tMin.setMin(tv3);
tMax=tv1;
tMax.setMax(tv2);
tMax.setMax(tv3);
if (tMin.x>bRadii.x)
continue;
if (tMax.x<-bRadii.x)
continue;
if (tMin.y>bRadii.y)
continue;
if (tMax.y<-bRadii.y)
continue;
if (tMin.z>bRadii.z)
continue;
if (tMax.z<-bRadii.z)
continue;
*/
{
// collision
hitface = i;
hitNormal = normal;
hitDepth = overlap/normal.len();
}
}
hitNormal,hitDepth;
return hitface!=-1;
}
bool CelAnimMesh::collideSphere( int frameIndex,
const Point3F & center, float radius,
CollisionSurfaceList & csl) const
{
AssertFatal( fFrames.size() > 0, "Shape must have at least one frame." );
AssertFatal( frameIndex >= 0 && frameIndex < fFrames.size(),
"TS::CelAnimMesh: frame index out of range" );
// get the frame struct:
const Frame *frm = &fFrames[frameIndex];
int fv = frm->fFirstVert;
const Point3F *pScale = &frm->fScale;
const Point3F *pOrigin = &frm->fOrigin;
bool gotHit=false;
int i;
Point3F v[3];
for (i=0;i<fFaces.size();i++)
{
const Face & theFace = fFaces[i];
fVerts[theFace.fVIP[0].fVertexIndex+fv].getPoint(v[0],*pScale,*pOrigin);
fVerts[theFace.fVIP[1].fVertexIndex+fv].getPoint(v[1],*pScale,*pOrigin);
fVerts[theFace.fVIP[2].fVertexIndex+fv].getPoint(v[2],*pScale,*pOrigin);
// build the normal and distance from origin of face plane
Point3F normal;
m_normal(v[0],v[1],v[2],normal);
float norm=normal.len();
if (norm==0.0f) // can happen if a face gets scrunched during animation
continue;
normal *= 1.0f/norm;
// distance from origin of plane and parallel plane containing sphere center
float dSphere = m_dot(normal,center);
float dPlane = m_dot(normal,v[0]);
float dist = dSphere-dPlane;
if (dist<0) // ignore back facing planes
continue;
if (dist>=radius) // whole plane outside of sphere
continue;
// intersection of face plane and sphere is a circle on face plane
// with center p (below) and radius circleRad (below)
Point3F p;
p.x = center.x - dist * normal.x;
p.y = center.y - dist * normal.y;
p.z = center.z - dist * normal.z;
float circleRad2 = radius*radius - dist*dist; // square of circle radius
// check to see if p is inside face (in case whole circle inside face)
if (m_pointInTriangle(p,normal,v[0],v[1],v[2]))
{
// add entry to cil
csl.increment();
CollisionSurface & info = csl.last();
info.surface=i;
info.material = theFace.fMaterial;
info.position = p;
info.normal = normal;
info.distance = dist;
gotHit=true;
continue;
}
// go through each edge, check to see if it intersects inside of circle
// if so, add collision info
for (int j=0;j<3;j++)
{
const Point3F & pivotVert = v[j];
Point3F edge;
edge.x = v[(j+1) % 3].x - pivotVert.x;
edge.y = v[(j+1) % 3].y - pivotVert.y;
edge.z = v[(j+1) % 3].z - pivotVert.z;
Point3F radialLine;
radialLine.x = p.x - pivotVert.x;
radialLine.y = p.y - pivotVert.y;
radialLine.z = p.z - pivotVert.z;
float edgeLen=edge.len();
if (edgeLen==0.0f)
continue;
edge *= 1.0f/edgeLen;
// get length of projection of radial line onto edge line
float projRadialToEdgeDist = m_dot(radialLine,edge);
// find point at which perp. to edge from circle center hits edge line
Point3F intPoint;
intPoint.x = pivotVert.x + projRadialToEdgeDist * edge.x;
intPoint.y = pivotVert.y + projRadialToEdgeDist * edge.y;
intPoint.z = pivotVert.z + projRadialToEdgeDist * edge.z;
float distToIntersect2 = (p.x-intPoint.x)*(p.x-intPoint.x) +
(p.y-intPoint.y)*(p.y-intPoint.y) +
(p.z-intPoint.z)*(p.z-intPoint.z);
// is whole line outside circle
if (distToIntersect2>circleRad2)
continue;
// length of half-cord formed by edge line
// float halfcord = sqrt(circleRad2 - distToIntersect2);
float halfcord2 = circleRad2 - distToIntersect2;
// check to see if line lands inside of circle
// if ( (projRadialToEdgeDist < 0.0f-halfcord) ||
// (projRadialToEdgeDist > halfcord+edgeLen) )
// continue;
if (projRadialToEdgeDist < 0.0f)
{
if (projRadialToEdgeDist*projRadialToEdgeDist>halfcord2)
continue;
}
else if (projRadialToEdgeDist>edgeLen)
{
float tmpF = projRadialToEdgeDist-edgeLen;
if (tmpF*tmpF>halfcord2)
continue;
}
// we know we hit now...
// where exactly did we hit, shift intersect point onto edge to form hitPoint
if (projRadialToEdgeDist < 0.0f)
projRadialToEdgeDist = 0.0f;
else if (projRadialToEdgeDist > edgeLen)
projRadialToEdgeDist = edgeLen;
Point3F hitPoint;
hitPoint.x = pivotVert.x + projRadialToEdgeDist * edge.x;
hitPoint.y = pivotVert.y + projRadialToEdgeDist * edge.y;
hitPoint.z = pivotVert.z + projRadialToEdgeDist * edge.z;
// add entry to cil
csl.increment();
CollisionSurface & info = csl.last();
info.surface=i;
info.material = theFace.fMaterial;
info.position = hitPoint;
info.normal = normal;
info.distance = dist;
gotHit=true;
break;
}
}
return gotHit;
}
bool CelAnimMesh::collideTube( int frameIndex, const Point3F & a, const Point3F &b, float radius,
CollisionSurface & cs, float minTime) const
{
minTime;
// hitTime holds the current smallest...
float hitTime = cs.time;
int hitFace = -1;
Point3F hitPoint;
AssertFatal( fFrames.size() > 0, "Shape must have at least one frame." );
AssertFatal( frameIndex >= 0 && frameIndex < fFrames.size(),
"TS::CelAnimMesh: frame index out of range" );
// get the frame struct:
const Frame *frm = &fFrames[frameIndex];
int fv = frm->fFirstVert;
const Point3F *pScale = &frm->fScale;
const Point3F *pOrigin = &frm->fOrigin;
Point3F tubeVect;
tubeVect.x = b.x - a.x;
tubeVect.y = b.y - a.y;
tubeVect.z = b.z - a.z;
float tubeLen = tubeVect.len();
float invTubeLen = 1.0f/tubeLen;
// tubeVect will hold unit length vector pointing down tube
tubeVect *= invTubeLen;
float vectDotA = m_dot(tubeVect,a);
// inverse radius squared for edgeInTube routine
float invRadius2 = 1.0f / (radius*radius);
int i;
workVerts.setSize(fnVertsPerFrame);
workRs.setSize(fnVertsPerFrame);
workTs.setSize(fnVertsPerFrame);
bool gotNormal;
for (i=0;i<fFaces.size();i++)
{
const Face & theFace = fFaces[i];
int idx1 = theFace.fVIP[0].fVertexIndex;
int idx2 = theFace.fVIP[1].fVertexIndex;
int idx3 = theFace.fVIP[2].fVertexIndex;
Point3F &v1=workVerts[idx1];
Point3F &v2=workVerts[idx2];
Point3F &v3=workVerts[idx3];
Point3F &R1=workRs[idx1];
Point3F &R2=workRs[idx2];
Point3F &R3=workRs[idx3];
float &t1=workTs[idx1];
float &t2=workTs[idx2];
float &t3=workTs[idx3];
if (!(v1Recycled&faceReuseFlags[i]))
{
fVerts[idx1+fv].getPoint(v1,*pScale,*pOrigin);
// distance of vertex down the tube
t1 = m_dot(v1,tubeVect) - vectDotA;
// projection of vertex onto tube cross-section (centered on origin)
R1.x = v1.x - a.x - t1 * tubeVect.x;
R1.y = v1.y - a.y - t1 * tubeVect.y;
R1.z = v1.z - a.z - t1 * tubeVect.z;
}
if (!(v2Recycled&faceReuseFlags[i]))
{
fVerts[idx2+fv].getPoint(v2,*pScale,*pOrigin);
// distance of vertex down the tube
t2 = m_dot(v2,tubeVect) - vectDotA;
// projection of vertex onto tube cross-section (centered on origin)
R2.x = v2.x - a.x - t2 * tubeVect.x;
R2.y = v2.y - a.y - t2 * tubeVect.y;
R2.z = v2.z - a.z - t2 * tubeVect.z;
}
if (!(v3Recycled&faceReuseFlags[i]))
{
fVerts[idx3+fv].getPoint(v3,*pScale,*pOrigin);
// distance of vertex down the tube
t3 = m_dot(v3,tubeVect) - vectDotA;
// projection of vertex onto tube cross-section (centered on origin)
R3.x = v3.x - a.x - t3 * tubeVect.x;
R3.y = v3.y - a.y - t3 * tubeVect.y;
R3.z = v3.z - a.z - t3 * tubeVect.z;
}
bool gotHit=false;
if (!(e1Recycled&faceReuseFlags[i]))
{
if (t1<=t2)
gotHit = edgeInTube(R1,R2,t1,t2,radius,invRadius2,invTubeLen,hitTime,hitPoint);
else
gotHit = edgeInTube(R2,R1,t2,t1,radius,invRadius2,invTubeLen,hitTime,hitPoint);
}
if (!(e2Recycled&faceReuseFlags[i]))
{
if (t2<=t3)
gotHit |= edgeInTube(R2,R3,t2,t3,radius,invRadius2,invTubeLen,hitTime,hitPoint);
else
gotHit |= edgeInTube(R3,R2,t3,t2,radius,invRadius2,invTubeLen,hitTime,hitPoint);
}
if (!(e3Recycled&faceReuseFlags[i]))
{
if (t3<=t1)
gotHit |= edgeInTube(R3,R1,t3,t1,radius,invRadius2,invTubeLen,hitTime,hitPoint);
else
gotHit |= edgeInTube(R1,R3,t1,t3,radius,invRadius2,invTubeLen,hitTime,hitPoint);
}
if (gotHit)
{
hitPoint.x += tubeLen * hitTime * tubeVect.x + a.x;
hitPoint.y += tubeLen * hitTime * tubeVect.y + a.y;
hitPoint.z += tubeLen * hitTime * tubeVect.z + a.z;
hitFace=i;
gotNormal=false;
}
// now check if tube goes through center of face w/o hitting any edges
if (m_pointInTriangle(Point3F(0.0f,0.0f,0.0f),tubeVect,R1,R2,R3))
{
// build the normal
Point3F normal;
m_normal(v1,v2,v3,normal);
// now we need to find hitTime
float d = m_dot(normal,v3); // distance of plane from origin
float denom = m_dot(normal,tubeVect) * tubeLen;
if (denom>=0.0f) // back face, we can ignore
continue;
float absT = d - m_dot(normal,a);
if (absT<=hitTime*denom) // denom is neg.
continue;
// ok, a real collision, set ci variables...
hitTime=absT/denom;
hitFace=i;
cs.normal=normal;
gotNormal=true;
hitPoint.x = a.x + hitTime * tubeLen * tubeVect.x;
hitPoint.y = a.y + hitTime * tubeLen * tubeVect.y;
hitPoint.z = a.z + hitTime * tubeLen * tubeVect.z;
}
}
if (hitFace>=0)
{
const Face & theFace = fFaces[hitFace];
if (!gotNormal)
{
Point3F &v1=workVerts[theFace.fVIP[0].fVertexIndex];
Point3F &v2=workVerts[theFace.fVIP[1].fVertexIndex];
Point3F &v3=workVerts[theFace.fVIP[2].fVertexIndex];
// build the normal
Point3F v13,v23;
v13.x = v1.x-v3.x;
v13.y = v1.y-v3.y;
v13.z = v1.z-v3.z;
v23.x = v2.x-v3.x;
v23.y = v2.y-v3.y;
v23.z = v2.z-v3.z;
m_cross(v23,v13,&cs.normal);
}
cs.material=theFace.fMaterial;
cs.surface=hitFace;
cs.time=hitTime;
cs.position=hitPoint;
// cs.distance ??
return true;
}
return false;
}
bool CelAnimMesh::collideLine( int frameIndex, const Point3F & a, const Point3F & b,
CollisionSurface & cs, float minTime ) const
{
float hitTime = cs.time;
int hitFace = -1; // if -1 on exit, then we didn't hit a face
AssertFatal( fFrames.size() > 0, "Shape must have at least one frame." );
AssertFatal( frameIndex >= 0 && frameIndex < fFrames.size(),
"TS::CelAnimMesh: frame index out of range" );
// get the frame struct:
const Frame *frm = &fFrames[frameIndex];
int fv = frm->fFirstVert;
const Point3F *pScale = &frm->fScale;
const Point3F *pOrigin = &frm->fOrigin;
Point3F vect;
vect.x = b.x - a.x;
vect.y = b.y - a.y;
vect.z = b.z - a.z;
// set up range of possible hitTimes (w/ hitTime holding the current smallest hit)
minTime *= 0.9f; // * 0.9f to account for rounding error
int i;
Point3F v1,v2,v3;
for (i=0;i<fFaces.size();i++)
{
const Face & theFace = fFaces[i];
fVerts[theFace.fVIP[0].fVertexIndex+fv].getPoint(v1,*pScale,*pOrigin);
fVerts[theFace.fVIP[1].fVertexIndex+fv].getPoint(v2,*pScale,*pOrigin);
fVerts[theFace.fVIP[2].fVertexIndex+fv].getPoint(v3,*pScale,*pOrigin);
// build the normal
Point3F normal;
m_normal(v1,v2,v3,normal);
// get distance from origin or plane
float d = m_dot(normal,v3); // distance of plane from origin
float denom = m_dot(normal,vect);
// if (denom==0.0f) // if we want to check all faces
if (denom>=0.0f) // check front facing faces only
continue;
float absT = d - m_dot(normal,a);
if (absT>=minTime*denom || absT<hitTime*denom) // denom negative
continue;
float t=absT/denom;
// intersection = a + hitTime*(b-a)
Point3F intersection;
intersection.x = a.x + t*vect.x;
intersection.y = a.y + t*vect.y;
intersection.z = a.z + t*vect.z;
if (m_pointInTriangle(intersection,normal,v1,v2,v3))
{
hitTime=t;
hitFace=i;
cs.normal=normal;
}
}
if (hitFace>=0)
{
cs.time=hitTime;
cs.surface=hitFace;
cs.material=fFaces[hitFace].fMaterial;
// cs.distance?
// normal already set
return true;
}
return false;
}
Vector4 Plane::GetVector() const {
return Vector4(m_normal(1),m_normal(2),m_normal(3),m_distance);
}
Vector4 Plane::GetNormal() const {
return Vector4(m_normal(1),m_normal(2),m_normal(3),0.0f);
}
