bool MFCollision_RaySphereTest(const MFVector& rayPos, const MFVector& rayDir, const MFVector& spherePos, float radius, MFRayIntersectionResult *pResult)
{
MFVector diff = rayPos - spherePos;
// calcuate the coefficients
float a = rayDir.MagSquared3();
float b = (2.0f*rayDir).Dot3(diff);
float c = diff.MagSquared3() - radius*radius;
// calculate the stuff under the root sign, if it's negative no (real) solutions exist
float d = b*b - 4.0f*a*c;
if(d < 0.0f) // this means ray misses cylinder
return false;
float root = MFSqrt(d);
float rcp2a = MFRcp(2.0f*a);
float t1 = (-b - root)*rcp2a;
float t2 = (-b + root)*rcp2a;
if(t2 < 0.0f || t1 > 1.0f)
return false;
if(pResult)
{
pResult->time = MFMax(t1, 0.0f);
pResult->surfaceNormal.Mad3(rayDir, pResult->time, diff);
pResult->surfaceNormal.Normalise3();
}
return true;
}
bool MFCollision_SphereSphereTest(const MFVector &pos1, float radius1, const MFVector &pos2, float radius2, MFCollisionResult *pResult)
{
MFVector diff = pos2 - pos1;
if(!pResult)
{
return diff.MagSquared3() < radius1*radius1 + radius2*radius2;
}
else
{
float length = diff.Magnitude3();
float totalRadius = radius1 + radius2;
pResult->bCollide = length < totalRadius;
if(pResult->bCollide)
{
pResult->depth = totalRadius - length;
pResult->normal = -diff.Normalise3();
pResult->intersectionPoint = diff * ((length / totalRadius) * (radius1 / radius2));
}
return pResult->bCollide;
}
}
MFMatrix& MFMatrix::PreciseTween(const MFMatrix& start, const MFMatrix& end, float time)
{
MFQuaternion q1, q2;
MFVector scale1, scale2;
MFVector trans;
q1 = start.GetRotationQ();
q2 = end.GetRotationQ();
scale1.x = start.GetXAxis().Magnitude3();
scale1.y = start.GetYAxis().Magnitude3();
scale1.z = start.GetZAxis().Magnitude3();
scale2.x = end.GetXAxis().Magnitude3();
scale2.y = end.GetYAxis().Magnitude3();
scale2.z = end.GetZAxis().Magnitude3();
trans = start.GetTrans();
q1.Slerp(q2, time);
trans.Lerp(end.GetTrans(), time);
scale1.Lerp(scale2, time);
SetRotationQ(q1);
Scale(scale1);
SetTrans3(trans);
return *this;
}
// culls backfaces
bool MFCollision_RayTriCullTest(const MFVector& rayPos, const MFVector& rayDir, const MFVector& p0, const MFVector& p1, const MFVector& p2, float *pT, float *pU, float *pV, MFVector *pIntersectionPoint)
{
MFVector edge1, edge2, tvec, pvec, qvec;
float det, inv_det;
float u, v;
/* find vectors for two edges sharing vert0 */
edge1 = p1 - p0;
edge2 = p2 - p0;
/* begin calculating determinant - also used to calculate U parameter */
pvec.Cross3(rayDir, edge2);
/* if determinant is near zero, ray lies in plane of triangle */
det = edge1.Dot3(pvec);
if (det < MFALMOST_ZERO)
return false;
/* calculate distance from vert0 to ray origin */
tvec = rayPos - p0;
/* calculate U parameter and test bounds */
u = tvec.Dot3(pvec);
if (u < 0.0f || u > det)
return false;
/* prepare to test V parameter */
qvec.Cross3(tvec, edge1);
/* calculate V parameter and test bounds */
v = rayDir.Dot3(qvec);
if (v < 0.0f || u + v > det)
return false;
/* calculate t, scale parameters, ray intersects triangle */
if(pIntersectionPoint || pT || pU)
{
inv_det = 1.0f / det;
u *= inv_det;
v *= inv_det;
if(pT) *pT = edge2.Dot3(qvec) * inv_det;
if(pU)
{
*pU = u;
*pV = v;
}
if(pIntersectionPoint)
{
*pIntersectionPoint = p0*(1.0f-u-v) + p1*u + p2*v;
}
}
return true;
}
bool MFCollision_PlaneTriTest(const MFVector& plane, const MFVector& p0, const MFVector& p1, const MFVector& p2, MFVector *pIntersectionPoint)
{
float t0 = p0.DotH(plane);
float t1 = p1.DotH(plane);
float t2 = p2.DotH(plane);
if(t0 <= 0.0f && t1 <= 0.0f && t2 <= 0)
return false;
if(t0 >= 0.0f && t1 >= 0.0f && t2 >= 0)
return false;
if(pIntersectionPoint)
{
// TODO: calculate point
}
return true;
}
bool MFCollision_RaySlabTest(const MFVector& rayPos, const MFVector& rayDir, const MFVector& plane, float slabHalfWidth, MFRayIntersectionResult *pResult)
{
float a = plane.Dot3(rayDir);
// if ray is parallel to plane
if(a > -MFALMOST_ZERO && a < MFALMOST_ZERO)
{
// TODO: this is intentionally BROKEN
// this is a near impossible case, and it adds a lot of junk to the function
/*
if(MFAbs(rayPos.DotH(plane)) <= slabHalfWidth)
{
if(pResult)
{
pResult->time = 0.0f;
}
return true;
}
*/
return false;
}
// otherwise we can do the conventional test
float inva = MFRcp(a);
float t = -rayPos.DotH(plane);
float t1 = (t + slabHalfWidth) * inva;
float t2 = (t - slabHalfWidth) * inva;
t = MFMin(t1, t2);
t2 = MFMax(t1, t2);
if(t > 1.0f || t2 < 0.0f)
return false;
if(pResult)
{
pResult->time = MFMax(t, 0.0f);
pResult->surfaceNormal = a > 0.0f ? -plane : plane;
}
return true;
}
void MFCollision_CalculateDynamicBoundingVolume(MFCollisionItem *pItem)
{
switch(pItem->pTemplate->type)
{
case MFCT_Mesh:
{
MFCollisionMesh *pMesh = (MFCollisionMesh*)pItem->pTemplate->pCollisionTemplateData;
MFBoundingVolume &vol = pItem->pTemplate->boundingVolume;
vol.min = vol.max = vol.boundingSphere = MakeVector(pMesh->pTriangles[0].verts[0], 0.0f);
for(int a=0; a<pMesh->numTris; a++)
{
MFCollisionTriangle &tri = pMesh->pTriangles[a];
for(int b=0; b<3; b++)
{
vol.min = MFMin(vol.min, tri.verts[b]);
vol.max = MFMin(vol.max, tri.verts[b]);
vol.min.w = vol.max.w = 0.0f;
// if point is outside bounding sphere
MFVector diff = tri.verts[b] - vol.boundingSphere;
float mag = diff.MagSquared3();
if(mag > vol.boundingSphere.w*vol.boundingSphere.w)
{
// fit sphere to include point
mag = MFSqrt(mag) - vol.boundingSphere.w;
mag *= 0.5f;
diff.Normalise3();
vol.boundingSphere.Mad3(diff, mag, vol.boundingSphere);
vol.boundingSphere.w += mag;
}
}
}
break;
}
default:
MFDebug_Assert(false, "Invalid item type");
}
}
bool MFCollision_SpherePlaneTest(const MFVector& spherePos, float radius, const MFVector& plane, MFCollisionResult *pResult)
{
if(!pResult)
{
return spherePos.DotH(plane) < radius;
}
else
{
float d = spherePos.DotH(plane);
pResult->bCollide = d < radius;
if(pResult->bCollide)
{
pResult->depth = radius - d;
pResult->normal = plane;
pResult->intersectionPoint.Mad3(pResult->normal, -(d + pResult->depth*0.5f), spherePos);
}
return pResult->bCollide;
}
}
bool MFCollision_TriTriTest(const MFVector& V0, const MFVector& V1, const MFVector& V2, const MFVector& U0, const MFVector& U1, const MFVector& U2)
{
MFVector E1, E2;
MFVector N1, N2;
MFVector D;
float d1,d2;
float du0,du1,du2,dv0,dv1,dv2;
float isect1[2], isect2[2];
float du0du1,du0du2,dv0dv1,dv0dv2;
short index;
float vp0,vp1,vp2;
float up0,up1,up2;
float bb,cc,max;
float a,b,c,x0,x1;
float d,e,f,y0,y1;
float xx,yy,xxyy,tmp;
/* compute plane equation of triangle(V0,V1,V2) */
E1 = V1-V0;
E2 = V2-V0;
N1.Cross3(E1, E2);
d1 = -N1.Dot3(V0);
/* plane equation 1: N1.X+d1=0 */
/* put U0,U1,U2 into plane equation 1 to compute signed distances to the plane*/
du0 = N1.Dot3(U0) + d1;
du1 = N1.Dot3(U1) + d1;
du2 = N1.Dot3(U2) + d1;
/* coplanarity robustness check */
if(fabsf(du0)<MFALMOST_ZERO) du0=0.0f;
if(fabsf(du1)<MFALMOST_ZERO) du1=0.0f;
if(fabsf(du2)<MFALMOST_ZERO) du2=0.0f;
du0du1 = du0*du1;
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;
N2.Cross3(E1, E2);
d2 = -N2.Dot3(U0);
/* plane equation 2: N2.X+d2=0 */
/* put V0,V1,V2 into plane equation 2 */
dv0 = N2.Dot3(V0) + d2;
dv1 = N2.Dot3(V1) + d2;
dv2 = N2.Dot3(V2) + d2;
if(fabsf(dv0)<MFALMOST_ZERO) dv0=0.0;
if(fabsf(dv1)<MFALMOST_ZERO) dv1=0.0;
if(fabsf(dv2)<MFALMOST_ZERO) dv2=0.0;
dv0dv1 = dv0*dv1;
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 */
D.Cross3(N1, N2);
/* compute and index to the largest component of D */
max = fabsf(D.x);
index = 0;
bb = fabsf(D.y);
cc = fabsf(D.z);
if(bb>max) max = bb, index = 1;
if(cc>max) max = cc, index = 2;
/* this is the simplified projection onto L*/
vp0 = V0[index];
vp1 = V1[index];
vp2 = V2[index];
up0 = U0[index];
up1 = U1[index];
up2 = U2[index];
/* compute interval for triangle 1 */
NEWCOMPUTE_INTERVALS(vp0,vp1,vp2,dv0,dv1,dv2,dv0dv1,dv0dv2,a,b,c,x0,x1);
/* compute interval for triangle 2 */
NEWCOMPUTE_INTERVALS(up0,up1,up2,du0,du1,du2,du0du1,du0du2,d,e,f,y0,y1);
xx=x0*x1;
yy=y0*y1;
xxyy=xx*yy;
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;
}
bool MFCollision_SweepSphereTriTest(const MFVector &sweepSpherePos, const MFVector &sweepSphereVelocity, float sweepSphereRadius, const MFCollisionTriangle &tri, MFSweepSphereResult *pResult)
{
MFRayIntersectionResult result;
// test the triangle surface
if(!MFCollision_RaySlabTest(sweepSpherePos, sweepSphereVelocity, tri.plane, sweepSphereRadius, &result))
return false;
MFVector intersection = sweepSpherePos + sweepSphereVelocity * result.time;
// test if intersection is inside the triangle
float dot0, dot1, dot2;
dot0 = intersection.DotH(tri.edgePlanes[0]);
// if(dot0 > sphereRadius)
// return false;
dot1 = intersection.DotH(tri.edgePlanes[1]);
// if(dot1 > sphereRadius)
// return false;
dot2 = intersection.DotH(tri.edgePlanes[2]);
// if(dot2 > sphereRadius)
// return false;
// test if intersection is inside the triangle face
if(dot0 >= 0.0f && dot1 >= 0.0f && dot2 >= 0.0f)
goto collision;
// test the 3 edges
if(dot0 < 0.0f)
{
if(MFCollision_RayCapsuleTest(sweepSpherePos, sweepSphereVelocity, tri.verts[0], tri.verts[1]-tri.verts[0], sweepSphereRadius, &result))
goto collision;
}
if(dot1 < 0.0f)
{
if(MFCollision_RayCapsuleTest(sweepSpherePos, sweepSphereVelocity, tri.verts[1], tri.verts[2]-tri.verts[1], sweepSphereRadius, &result))
goto collision;
}
if(dot2 < 0.0f)
{
if(MFCollision_RayCapsuleTest(sweepSpherePos, sweepSphereVelocity, tri.verts[2], tri.verts[0]-tri.verts[2], sweepSphereRadius, &result))
goto collision;
}
return false;
collision:
if(result.time == 0.0f)
{
// this is for double sided collision.
float dot = sweepSpherePos.DotH(tri.plane);
float amountResolve = sweepSphereRadius - MFAbs(dot);
pResult->intersectionReaction = result.surfaceNormal * amountResolve;
}
else
pResult->intersectionReaction = MFVector::zero;
pResult->surfaceNormal = result.surfaceNormal;
pResult->time = result.time;
return true;
}
bool MFCollision_RayCylinderTest(const MFVector& rayPos, const MFVector& rayDir, const MFVector& cylinderPos, const MFVector& cylinderDir, float cylinderRadius, bool capped, MFRayIntersectionResult *pResult, float *pCylinderTime)
{
MFVector local = rayPos - cylinderPos;
float rayD = rayDir.Dot3(cylinderDir);
float T0 = local.Dot3(cylinderDir);
// bring T0 into 0.0-1.0 range
float invMagSq = MFRcp(cylinderDir.MagSquared3());
rayD *= invMagSq;
T0 *= invMagSq;
// calculate some intermediate vectors
MFVector v1 = rayDir - rayD*cylinderDir;
MFVector v2 = local - T0*cylinderDir;
// calculate coeff in quadratic formula
float a = v1.MagSquared3();
float b = (2.0f*v1).Dot3(v2);
float c = v2.MagSquared3() - cylinderRadius*cylinderRadius;
// calculate the stuff under the root sign, if it's negative no (real) solutions exist
float d = b*b - 4.0f*a*c;
if(d < 0.0f) // this means ray misses cylinder
return false;
float root = MFSqrt(d);
float rcp2a = MFRcp(2.0f*a);
float t1 = (-b - root)*rcp2a;
float t2 = (-b + root)*rcp2a;
if(t1 > 1.0f || t2 < 0.0f)
return false; // the cylinder is beyond the ray..
if(capped || pCylinderTime || pResult)
{
float t = MFMax(t1, 0.0f);
// get the t for the cylinders ray
MFVector intersectedRay;
intersectedRay.Mad3(rayDir, t, local);
float ct = intersectedRay.Dot3(cylinderDir) * invMagSq;
if(capped && (ct < 0.0f || ct > 1.0f))
{
// we need to test the caps
// TODO: this is REALLY slow!! can be majorly improved!!
// generate a plane for the cap
MFVector point, plane;
if(rayD > 0.0f)
{
// the near one
point = cylinderPos;
plane = MFCollision_MakePlaneFromPointAndNormal(point, -cylinderDir);
}
else
{
// the far one
point = cylinderPos + cylinderDir;
plane = MFCollision_MakePlaneFromPointAndNormal(point, cylinderDir);
}
// test the ray against the plane
bool collide = MFCollision_RayPlaneTest(rayPos, rayDir, plane, pResult);
if(collide)
{
// calculate the intersection point
intersectedRay.Mad3(rayDir, pResult->time, rayPos);
intersectedRay.Sub3(intersectedRay, point);
// and see if its within the cylinders radius
if(intersectedRay.MagSquared3() <= cylinderRadius * cylinderRadius)
{
return true;
}
}
return false;
}
if(pResult)
{
pResult->time = t;
pResult->surfaceNormal.Mad3(cylinderDir, -ct, intersectedRay);
pResult->surfaceNormal.Normalise3();
}
if(pCylinderTime)
{
*pCylinderTime = ct;
}
}
return true;
}
MF_API void MFCollision_BuildField(MFCollisionItem *pField)
{
MFCollisionField *pFieldData = (MFCollisionField*)pField->pTemplate;
int numItems = pFieldData->itemList.GetLength();
if(numItems <= 0)
{
MFDebug_Warn(4, "EmptyField can not be generated.");
return;
}
// find the min and max range of the objects
MFVector fieldMin = MakeVector(10e+30f), fieldMax = MakeVector(-10e+30f);
MFCollisionItem **ppI = pFieldData->itemList.Begin();
while(*ppI)
{
MFCollisionItem *pI = *ppI;
MFCollisionTemplate *pT = pI->pTemplate;
MFVector tMin = ApplyMatrixH(pT->boundingVolume.min, pI->worldPos);
MFVector tMax = ApplyMatrixH(pT->boundingVolume.max, pI->worldPos);
fieldMin = MFMin(fieldMin, tMin);
fieldMax = MFMax(fieldMax, tMax);
ppI++;
}
pFieldData->fieldMin = fieldMin;
pFieldData->fieldMax = fieldMin;
MFVector numCells;
MFVector fieldRange = fieldMax - fieldMin;
numCells.Rcp3(pFieldData->cellSize);
numCells.Mul3(fieldRange, numCells);
pFieldData->width = (int)MFCeil(numCells.x);
pFieldData->height = (int)MFCeil(numCells.y);
pFieldData->depth = (int)MFCeil(numCells.z);
// this is TOTALLY broken!! .. if a big object lies in many cell's, it could easilly overflow the array.
int totalCells = pFieldData->width * pFieldData->height * pFieldData->depth;
int numPointers = totalCells * 2 + numItems * 16;
MFCollisionItem **ppItems = (MFCollisionItem**)MFHeap_Alloc(sizeof(MFCollisionItem*) * numPointers);
pFieldData->pppItems = (MFCollisionItem***)ppItems;
ppItems += totalCells;
for(int z=0; z<pFieldData->depth; z++)
{
for(int y=0; y<pFieldData->height; y++)
{
for(int x=0; x<pFieldData->width; x++)
{
pFieldData->pppItems[z*pFieldData->height*pFieldData->width + y*pFieldData->width + x] = ppItems;
MFVector thisCell = fieldMin + pFieldData->cellSize * MakeVector((float)x, (float)y, (float)z);
MFVector thisCellEnd = thisCell + pFieldData->cellSize;
MFCollisionItem **ppI = pFieldData->itemList.Begin();
while(*ppI)
{
MFCollisionItem *pI = *ppI;
MFCollisionTemplate *pT = pI->pTemplate;
// if this item fits in this cell, insert it into this cells list.
MFVector tMin = ApplyMatrixH(pT->boundingVolume.min, pI->worldPos);
MFVector tMax = ApplyMatrixH(pT->boundingVolume.max, pI->worldPos);
// test of bounding boxes overlap
if(MFCollision_TestAABB(tMin, tMax, thisCell, thisCellEnd))
{
*ppItems = pI;
++ppItems;
}
ppI++;
}
*ppItems = NULL;
++ppItems;
}
}
}
MFHeap_ValidateMemory(pFieldData->pppItems);
}
void MFRenderer_ClearScreen(uint32 flags)
{
MFCALLSTACKc;
pd3dDevice->Clear(0, NULL, ((flags&CS_Colour) ? D3DCLEAR_TARGET : NULL)|((flags&CS_ZBuffer) ? D3DCLEAR_ZBUFFER : NULL)|((flags&CS_Stencil) ? D3DCLEAR_STENCIL : NULL), gClearColour.ToPackedColour(), 1.0f, 0);
}
void MFRenderer_ClearScreen(MFRenderClearFlags flags, MFVector colour, float z, int stencil)
{
MFCALLSTACKc;
pd3dDevice->Clear(0, NULL, ((flags&CS_Colour) ? D3DCLEAR_TARGET : NULL)|((flags&CS_ZBuffer) ? D3DCLEAR_ZBUFFER : NULL)|((flags&CS_Stencil) ? D3DCLEAR_STENCIL : NULL), colour.ToPackedColour(), z, stencil);
}
MF_API void MFParticleSystem_AddParticle(MFParticleEmitter *pEmitter)
{
MFParticleEmitterParameters *pE = &pEmitter->params;
MFParticleSystem *pParticleSystem = pE->pParticleSystem;
MFParticle *pNew = NULL;
if(pParticleSystem->particles.GetLength() < pParticleSystem->params.maxActiveParticles)
pNew = pParticleSystem->particles.Create();
if(pNew)
{
MFParticleParameters *pP = &pParticleSystem->params;
pNew->colour = pP->colour;
pNew->life = pP->life;
pNew->rot = 0.0f;
pNew->size = pP->size;
switch(pE->type)
{
case MFET_Point:
pNew->pos = pE->position.GetTrans();
break;
case MFET_Sphere:
case MFET_Disc:
{
MFVector offset;
do
{
offset = MakeVector(MFRand_Range(-pE->radius, pE->radius), MFRand_Range(-pE->radius, pE->radius), MFRand_Range(-pE->radius, pE->radius));
}
while(offset.MagSquared3() > pE->radius*pE->radius);
if(pE->type == MFET_Disc)
{
// flatten it on to the disc
float dist = offset.Dot3(pE->position.GetYAxis());
offset -= pE->position.GetYAxis()*dist;
}
pNew->pos = pE->position.GetTrans() + offset;
break;
}
}
switch(pE->behaviour)
{
case MFEB_Direction:
pNew->velocity.Normalise3(pE->startVector);
break;
case MFEB_TargetAttract:
pNew->velocity.Normalise3(pE->startVector - pE->position.GetTrans());
break;
case MFEB_TargetRepel:
pNew->velocity.Normalise3(pE->position.GetTrans() - pE->startVector);
break;
}
pNew->velocity *= pE->velocity + MFRand_Range(-pE->velocityScatter, pE->velocityScatter);
if(pE->directionScatter)
{
MFVector scatter;
do
{
scatter = MakeVector(MFRand_Range(-1, 1), MFRand_Range(-1, 1), MFRand_Range(-1, 1));
float dist = scatter.Dot3(pE->position.GetYAxis());
scatter -= pE->position.GetYAxis()*dist;
}
while(scatter.MagSquared3() < 0.000001f);
scatter.Normalise3();
MFMatrix scatterMat;
scatterMat.SetRotation(scatter, MFRand_Unit()*pE->directionScatter);
pNew->velocity = ApplyMatrixH(pNew->velocity, scatterMat);
}
}
}
