public bool IntersectPlane(out MyVector intersectionPoint, out MyVector intersectionNormal, out float collisionTime, MyVector startPoint, MyVector endPoint, float dt, CollisionPlane plane)
{
intersectionPoint = new MyVector();
intersectionNormal = -plane.Normal;
MyVector rVelocity = (endPoint - startPoint) / dt;
MyVector pVel = (rVelocity * plane.Normal) * plane.Normal;
MyVector translate = (endPoint - startPoint);
collisionTime = 0;
float speed;
float nPoints = 0;
for (int i = 0; i < m_hardPoints.Length; i++)
{
if ((m_hardPoints[i] - endPoint) * pVel > 0)
{
//the line doesn't cross the triangle
continue;
}
speed = pVel * (m_hardPoints[i] - startPoint);
if (speed < -0.04)
continue;
if (nPoints > 0 && speed > collisionTime)
continue;
if (nPoints == 0)
{
nPoints = 1;
intersectionPoint = m_hardPoints[i];
collisionTime = speed;
}
else if (speed < collisionTime)
{
nPoints = 1;
intersectionPoint = m_hardPoints[i];
collisionTime = speed;
}
else if (speed == collisionTime && nPoints > 0)
{
nPoints++;
intersectionPoint.Add(m_hardPoints[i]);
}
}
if (nPoints > 1)
{
intersectionPoint.Divide(nPoints);
}
return (nPoints > 0);
}
public bool IntersectSphere(out MyVector intersectionPoint, out MyVector intersectionNormal,
out float collisionTime, MyVector startPoint, MyVector endPoint, float dt, CollisionSphere sphere)
{
intersectionPoint = new MyVector();
intersectionNormal = new MyVector();
collisionTime = 0;
MyVector rVelocity = (endPoint - startPoint) / dt;
MyVector translate = (endPoint - startPoint);
int nPoints = 0;
//bool status = false;
MyVector iPoint;
MyVector n1, n2, n3;
MyVector sPoint;
MyVector ePoint;
float dist, speed, d1, d2, d3;
// check for face collision
for (int i = 0; i < m_faces.Length; i++)
{
sPoint = startPoint - m_faces[i].n * sphere.Radius;
ePoint = endPoint - m_faces[i].n * sphere.Radius;
if ((ePoint - m_vertices[m_faces[i].v1]) * m_faces[i].n > 0)
{
//the line doesn't cross the triangle
continue;
}
dist = m_faces[i].n * (sPoint - m_vertices[m_faces[i].v1]);
speed = -dist / (m_faces[i].n * rVelocity);
if (speed < 0)
{
if (translate * (m_faces[i].n)<0)
{
if (dist < -0.5f)
continue;
}
else
continue;
}
if (nPoints > 0 && speed > collisionTime)
continue;
iPoint = sPoint + speed * rVelocity;
//3 planes around triangle
//plane1
n1 = ((m_vertices[m_faces[i].v2] - m_vertices[m_faces[i].v1]) ^ m_faces[i].n).Normalize();
d1 = n1 * (m_vertices[m_faces[i].v1]);
//plane2
n2 = ((m_vertices[m_faces[i].v3] - m_vertices[m_faces[i].v2]) ^ m_faces[i].n).Normalize();
d2 = n2 * (m_vertices[m_faces[i].v2]);
//plane3
n3 = ((m_vertices[m_faces[i].v1] - m_vertices[m_faces[i].v3]) ^ m_faces[i].n).Normalize();
d3 = n3 * (m_vertices[m_faces[i].v3]);
float x1 = n1 * iPoint - d1;
float x2 = n2 * iPoint - d2;
float x3 = n3 * iPoint - d3;
if (x1 <= 0 && x2 <= 0 && x3 <= 0)
{
//if (status == false)
//{
// status = true;
// intersectionPoint = iPoint;
// intersectionNormal = m_faces[i].n;
// collisionTime = speed;
//}
//else if (speed < collisionTime)
//{
// intersectionPoint = iPoint;
// intersectionNormal = m_faces[i].n;
// collisionTime = speed;
//}
if (nPoints == 0)
{
nPoints = 1;
intersectionPoint = iPoint;
intersectionNormal = m_faces[i].n;
collisionTime = speed;
}
else if (speed < collisionTime)
{
nPoints = 1;
intersectionPoint = iPoint;
intersectionNormal = m_faces[i].n;
collisionTime = speed;
}
else if (speed == collisionTime && nPoints > 0)
{
nPoints++;
intersectionPoint.Add(iPoint);
intersectionNormal.Add(m_faces[i].n);
}
}
}
//if (nPoints > 1)
//{
// intersectionPoint.Divide(nPoints);
// intersectionNormal.Divide(nPoints);
//}
//if (status == true)
//{
// return true;
//}
//status = false;
// no face collision
//check for edge collision
foreach (CEdge e in m_edges)
{
MyVector DV, DVP;
DV = m_vertices[e.v2] - m_vertices[e.v1];
DVP = m_vertices[e.v1] - startPoint;
double a = (double)DV.LengthSq * (double)rVelocity.LengthSq - (double)(DV * rVelocity) * (double)(DV * rVelocity);
double b = -2 * (double)((double)DV.LengthSq * (double)(DVP * rVelocity) - (double)(DVP * DV) * (double)(rVelocity * DV));
double c = (double)DV.LengthSq * (double)DVP.LengthSq - (double)(DV * DVP) * (double)(DV * DVP) - (double)(sphere.Radius * sphere.Radius) * (double)DV.LengthSq;
double delta = b * b - 4 * a * c;
if (delta >= 0)
{
double t;
if (a > 0)
{
t = (-b - Math.Sqrt(delta)) / (2 * a);
}
else
{
t = (-b + Math.Sqrt(delta)) / (2 * a);
}
if (t < -0.04)
continue;
if (nPoints > 0 && t > collisionTime)
continue;
iPoint = startPoint + (float)t * rVelocity;
iPoint.Add((DV ^ (DVP ^ DV)).Normalize() * sphere.Radius);
if ((iPoint - m_vertices[e.v1]) * DV >= 0 && (iPoint - m_vertices[e.v2]) * DV <= 0)
{
//intersectionPoint = iPoint;
//intersectionNormal = startPoint - intersectionPoint;
//intersectionNormal.Normalize();
//status = true;
//collisionTime = t;
if (nPoints == 0)
{
nPoints = 1;
intersectionPoint = iPoint;
intersectionNormal = startPoint - intersectionPoint;
intersectionNormal.Normalize();
collisionTime = (float)t;
}
else if (t < collisionTime)
{
nPoints = 1;
intersectionPoint = iPoint;
intersectionNormal = startPoint - intersectionPoint;
intersectionNormal.Normalize();
collisionTime = (float)t;
}
else if (t == collisionTime && nPoints > 0)
{
nPoints++;
intersectionPoint.Add(iPoint);
intersectionNormal.Add((startPoint - intersectionPoint).Normalize());
}
}
}
}
//if (status == true)
// return true;
//status = false;
//no edge collision
//check for vertex collision
for (int i = 0; i < m_vertices.Length; i++)
{
MyVector DVP;
DVP = m_vertices[i] - startPoint;
double a = rVelocity.LengthSq;
double b = -2 * (DVP * rVelocity);
double c = DVP.LengthSq - (sphere.Radius * sphere.Radius);
double delta = b * b - 4 * a * c;
if (delta >= 0)
{
double t;
if (a > 0)
{
t = (-b - Math.Sqrt(delta)) / (2 * a);
}
else
{
t = (-b + Math.Sqrt(delta)) / (2 * a);
}
if (t < -0.04)
continue;
if (nPoints > 0 && t > collisionTime)
continue;
//if (status == false || (status == true && collisionTime > t))
//{
// intersectionPoint = m_vertices[i];
// intersectionNormal = startPoint - intersectionPoint;
// intersectionNormal.Normalize();
// status = true;
// collisionTime = (float)t;
//}
if (nPoints == 0)
{
nPoints = 1;
intersectionPoint = m_vertices[i];
intersectionNormal = startPoint - intersectionPoint;
intersectionNormal.Normalize();
collisionTime = (float)t;
}
else if (t < collisionTime)
{
nPoints = 1;
intersectionPoint = m_vertices[i];
intersectionNormal = startPoint - intersectionPoint;
intersectionNormal.Normalize();
collisionTime = (float)t;
}
else if (t == collisionTime && nPoints > 0)
{
nPoints++;
intersectionPoint.Add(m_vertices[i]);
intersectionNormal.Add((startPoint - intersectionPoint).Normalize());
}
}
}
if (nPoints > 1)
{
intersectionPoint.Divide(nPoints);
intersectionNormal.Normalize();
}
return (nPoints > 0);
}
public bool IntersectMesh(out MyVector intersectionPoint, out MyVector intersectionNormal, out float collisionTime, MyVector startPoint, MyVector endPoint, float dt, CollisionMesh mesh)
{
intersectionPoint = new MyVector();
intersectionNormal = new MyVector();
MyVector translate = (endPoint - startPoint);
collisionTime = 0;
int nPoints = 0;
MyVector rVelocity=new MyVector();
MyVector rMove = endPoint - startPoint;
MyVector iPoint;
MyVector n1, n2, n3;
float dist, speed, d1, d2, d3;
for (int v = 0; v < mesh.m_hardPoints.Length; v++)
{
MyVector sPoint = startPoint + mesh.m_hardPoints[v];
MyVector axis = mesh.MeshRotation;
axis.Normalize();
MyQuaternion rot = MyQuaternion.FromAxisAngle((dt * mesh.MeshRotation).Length, axis);
//endPoint.Rotate(rot);
//endPoint += relativeVelocity * dt;
MyVector tmp=mesh.m_hardPoints[v];
tmp.Rotate(rot);
MyVector ePoint = startPoint + tmp + rMove;
rVelocity = (ePoint - sPoint) / dt;
translate = (ePoint - sPoint);
for (int i = 0; i < m_faces.Length; i++)
{
if ((ePoint - m_vertices[m_faces[i].v1]) * m_faces[i].n > 0)
{
//the line doesn't cross the triangle
continue;
}
dist = m_faces[i].n * (sPoint - m_vertices[m_faces[i].v1]);
speed = -dist / (m_faces[i].n * rVelocity);
if (speed < 0)
{
if (translate * (m_faces[i].n)<0)
{
if (dist < -0.5f)
continue;
}
else
continue;
}
iPoint = sPoint + speed * rVelocity;
//3 planes around triangle
//plane1
n1 = ((m_vertices[m_faces[i].v2] - m_vertices[m_faces[i].v1]) ^ m_faces[i].n).Normalize();
d1 = n1 * (m_vertices[m_faces[i].v1]);
//plane2
n2 = ((m_vertices[m_faces[i].v3] - m_vertices[m_faces[i].v2]) ^ m_faces[i].n).Normalize();
d2 = n2 * (m_vertices[m_faces[i].v2]);
//plane3
n3 = ((m_vertices[m_faces[i].v1] - m_vertices[m_faces[i].v3]) ^ m_faces[i].n).Normalize();
d3 = n3 * (m_vertices[m_faces[i].v3]);
float x1 = n1 * iPoint - d1;
float x2 = n2 * iPoint - d2;
float x3 = n3 * iPoint - d3;
if (x1 <= 0 && x2 <= 0 && x3 <= 0)
{
if (nPoints == 0)
{
nPoints = 1;
intersectionPoint = iPoint;
intersectionNormal = m_faces[i].n;
collisionTime = speed;
}
else if (speed < collisionTime)
{
nPoints = 1;
intersectionPoint = iPoint;
intersectionNormal = m_faces[i].n;
collisionTime = speed;
}
else if (speed == collisionTime && nPoints > 0)
{
nPoints++;
intersectionPoint.Add(iPoint);
intersectionNormal.Add(m_faces[i].n);
}
}
}
}
//second
//for (int v = 0; v < m_hardPoints.Length; v++)
//{
// //MyVector sPoint = -startPoint + m_hardPoints[v];
// //MyVector ePoint = -endPoint + m_hardPoints[v];
// MyVector sPoint = -startPoint + m_hardPoints[v];
// MyVector axis = -mesh.MeshRotation;
// axis.Normalize();
// MyQuaternion rot = MyQuaternion.FromAxisAngle((dt * mesh.MeshRotation).Length, axis);
// //endPoint.Rotate(rot);
// //endPoint += relativeVelocity * dt;
// MyVector tmp = m_hardPoints[v];
// tmp.Rotate(rot);
// MyVector ePoint = -startPoint + tmp - rMove;
// rVelocity = (ePoint - sPoint) / dt;
// translate = (ePoint - sPoint);
// for (int i = 0; i < mesh.m_faces.Length; i++)
// {
// if ((ePoint - mesh.m_vertices[mesh.m_faces[i].v1]) * mesh.m_faces[i].n > 0)
// {
// //the line doesn't cross the triangle
// continue;
// }
// dist = mesh.m_faces[i].n * (sPoint - mesh.m_vertices[mesh.m_faces[i].v1]);
// speed = -dist / (mesh.m_faces[i].n * (-rVelocity));
// if (speed < 0)
// {
// if (translate * (mesh.m_faces[i].n) < 0)
// {
// if (speed < -0.04)
// continue;
// }
// else
// continue;
// }
// iPoint = sPoint + speed * (-rVelocity);
// //3 planes around triangle
// //plane1
// n1 = ((mesh.m_vertices[mesh.m_faces[i].v2] - mesh.m_vertices[mesh.m_faces[i].v1]) ^ mesh.m_faces[i].n).Normalize();
// d1 = n1 * (mesh.m_vertices[mesh.m_faces[i].v1]);
// //plane2
// n2 = ((mesh.m_vertices[mesh.m_faces[i].v3] - mesh.m_vertices[mesh.m_faces[i].v2]) ^ mesh.m_faces[i].n).Normalize();
// d2 = n2 * (mesh.m_vertices[mesh.m_faces[i].v2]);
// //plane3
// n3 = ((mesh.m_vertices[mesh.m_faces[i].v1] - mesh.m_vertices[mesh.m_faces[i].v3]) ^ mesh.m_faces[i].n).Normalize();
// d3 = n3 * (mesh.m_vertices[mesh.m_faces[i].v3]);
// float x1 = n1 * iPoint - d1;
// float x2 = n2 * iPoint - d2;
// float x3 = n3 * iPoint - d3;
// if (x1 <= 0 && x2 <= 0 && x3 <= 0)
// {
// if (nPoints == 0)
// {
// nPoints = 1;
// intersectionPoint = iPoint + startPoint;
// intersectionNormal = -mesh.m_faces[i].n;
// collisionTime = speed;
// }
// else if (speed < collisionTime)
// {
// nPoints = 1;
// intersectionPoint = iPoint + startPoint;
// intersectionNormal = -mesh.m_faces[i].n;
// collisionTime = speed;
// }
// else if (speed == collisionTime && nPoints > 0)
// {
// nPoints++;
// intersectionPoint.Add(iPoint + startPoint);
// intersectionNormal.Add(-mesh.m_faces[i].n);
// }
// }
// }
//}
if (nPoints > 1)
{
intersectionPoint.Divide(nPoints);
intersectionNormal.Normalize();
}
return (nPoints > 0);
}
