void Octree::UpdateObject(CollisionMesh& object)
{
Partition* partition = object.GetPartition();
Partition* newPartition = nullptr;
assert(partition);
if(!IsAllInsidePartition(object, *partition))
{
// Move upwards until object is fully inside a single partition
Partition* parent = partition->GetParent();
while(parent && !IsAllInsidePartition(object, *parent))
{
parent = parent->GetParent();
}
// Will be in found partition or one of the children
newPartition = FindPartition(object, parent ? *parent : *m_octree);
}
else
{
// Can only be in partition and partition's children
newPartition = FindPartition(object, *partition);
}
assert(newPartition);
if(newPartition != partition)
{
// connect object and new partition together
partition->RemoveNode(object);
newPartition->AddNode(object);
object.SetPartition(newPartition);
}
}
D3DXVECTOR3 CollisionSolver::GetMinkowskiSumEdgePoint(const D3DXVECTOR3& direction,
const CollisionMesh& particle,
const CollisionMesh& hull)
{
return FindFurthestPoint(particle.GetVertices(), direction) -
FindFurthestPoint(hull.GetVertices(), -direction);
}
gep::CollisionMesh* gep::CollisionMeshFileLoader::loadResource(CollisionMesh* pInPlace)
{
CollisionMesh* result = nullptr;
bool isInPlace = true;
if (pInPlace == nullptr)
{
result = new CollisionMesh();
isInPlace = false;
}
auto* havokLoader = g_resourceManager.getHavokResourceLoader();
auto* container = havokLoader->load(m_path.c_str());
GEP_ASSERT(container != nullptr, "Could not load asset! %s", m_path.c_str());
if (container)
{
auto* physicsData = reinterpret_cast<hkpPhysicsData*>(container->findObjectByType(hkpPhysicsDataClass.getName()));
GEP_ASSERT(physicsData != nullptr, "Unable to load physics data!");
if (physicsData)
{
const auto& physicsSystems = physicsData->getPhysicsSystems();
GEP_ASSERT(physicsSystems.getSize() == 1, "Wrong number of physics systems!");
auto body = physicsSystems[0]->getRigidBodies()[0];
auto hkShape = body->getCollidable()->getShape();
auto shape = conversion::hk::from(const_cast<hkpShape*>(hkShape));
auto type = shape->getShapeType();
if ( type == hkcdShapeType::BV_COMPRESSED_MESH ||
type == hkcdShapeType::CONVEX_VERTICES )
{
hkTransform transform = body->getTransform();
transform.getRotation().setAxisAngle(hkVector4(0.0f, 0.0f, 1.0f), GetPi<float>::value());
auto hkTransformShape = new hkpTransformShape(hkShape, transform);
hkTransformShape->addReference();
shape = GEP_NEW(m_pAllocator, HavokShape_Transform)(hkTransformShape);
//auto meshShape = static_cast<HavokMeshShape*>(shape);
//Transform* tempTrans = new Transform();
//conversion::hk::from(transform, *tempTrans);
//
//// Since havok content tools are buggy (?) and no custom transformation can be applied,
//// we have to convert into our engine's space by hand.
//// TODO: Ensure, that this transformation is correct in every case
//tempTrans->setRotation(tempTrans->getRotation() * Quaternion(vec3(1,0,0),180));
//meshShape->setTransform(tempTrans);
}
result->setShape(shape);
}
}
return result;
}
Vector3 EdgeNormal(const CollisionMesh& m,int tri,int e)
{
Assert(!m.triNeighbors.empty());
Vector3 n=m.TriangleNormal(tri);
if(m.triNeighbors[tri][e] != -1) {
n += m.TriangleNormal(m.triNeighbors[tri][e]);
n.inplaceNormalize();
}
return m.currentTransform.R*n;
}
bool CollisionSolver::AreConvexHullsColliding(const CollisionMesh& particle,
const CollisionMesh& hull,
Simplex& simplex)
{
// If two convex hulls have collided, the Minkowski Sum A + (-B) of both
// hulls will contain the origin. Reference from 'Proximity Queries and
// Penetration Depth Computation on 3D Game Objects' by Gino van den Bergen
// http://graphics.stanford.edu/courses/cs468-01-fall/Papers/van-den-bergen.pdf
const std::vector<D3DXVECTOR3>& particleVertices = particle.GetVertices();
const std::vector<D3DXVECTOR3>& hullVertices = hull.GetVertices();
// Determine an initial point for the simplex
const int initialIndex = 0;
D3DXVECTOR3 direction = particleVertices[initialIndex] - hullVertices[initialIndex];
D3DXVECTOR3 lastEdgePoint = GetMinkowskiSumEdgePoint(direction, particle, hull);
simplex.AddPoint(lastEdgePoint);
direction = -direction;
int iteration = 0;
bool collisionFound = false;
bool collisionPossible = true;
const int maxIterations = 20;
// Iteratively create a simplex within the Minkowski Sum Hull
while(iteration < maxIterations && !collisionFound && collisionPossible)
{
++iteration;
lastEdgePoint = GetMinkowskiSumEdgePoint(direction, particle, hull);
simplex.AddPoint(lastEdgePoint);
if(D3DXVec3Dot(&lastEdgePoint, &direction) <= 0)
{
// New edge point of simplex is not past the origin.
collisionPossible = false;
}
else if(simplex.IsLine())
{
SolveLineSimplex(simplex, direction);
}
else if(simplex.IsTriPlane())
{
SolvePlaneSimplex(simplex, direction);
}
else if(simplex.IsTetrahedron())
{
collisionFound = SolveTetrahedronSimplex(simplex, direction);
}
}
return collisionFound;
}
Vector3 EdgeNormal(const CollisionMesh& m,int tri,int e)
{
if(m.triNeighbors.empty()) {
fprintf(stderr,"EdgeNormal: Warning, mesh is not properly initialized with triNeighbors\n");
return Vector3(0.0);
}
Assert(!m.triNeighbors.empty());
Vector3 n=m.TriangleNormal(tri);
if(m.triNeighbors[tri][e] != -1) {
n += m.TriangleNormal(m.triNeighbors[tri][e]);
n.inplaceNormalize();
}
return m.currentTransform.R*n;
}
void CollisionSolver::SolveObjectCollision(CollisionMesh& particle,
const CollisionMesh& object)
{
if(particle.IsDynamic())
{
if(object.GetShape() == Geometry::SPHERE)
{
SolveParticleSphereCollision(particle, object);
}
else
{
SolveParticleHullCollision(particle, object);
}
}
}
//Returns a contact normal for the closest point to the triangle t. p is the point on the triangle.
//The direction is the one in which triangle 1 can move to get away from closestpt
Vector3 ContactNormal(const CollisionMesh& m,const Vector3& p,int t,const Vector3& closestPt)
{
Triangle3D tri;
m.GetTriangle(t,tri);
Vector3 b=tri.barycentricCoords(p);
int type=FeatureType(b);
switch(type) {
case 1: //pt
//get the triangle normal
{
Vector3 n = VertexNormal(m,t,VertexIndex(b));
n.inplaceNegative();
return n;
}
break;
case 2: //edge
{
int e = EdgeIndex(b);
Vector3 n = EdgeNormal(m,t,e);
n.inplaceNegative();
return n;
}
break;
case 3: //face
return m.currentTransform.R*(-tri.normal());
}
static int warnedCount = 0;
if(warnedCount % 10000 == 0)
printf("ODECustomMesh: Warning, degenerate triangle, types %d\n",type);
warnedCount++;
//AssertNotReached();
return Vector3(Zero);
}
void Octree::IterateOctree(CollisionMesh& node)
{
if(m_iteratorFn)
{
auto& partition = *node.GetPartition();
IterateUpOctree(node, partition);
IterateDownOctree(node, partition);
}
}
void Octree::AddObject(CollisionMesh& object)
{
Partition* partition = FindPartition(object, *m_octree);
assert(partition);
// connect object and new partition together
partition->AddNode(object);
object.SetPartition(partition);
}
Vector3 VertexNormal(const CollisionMesh& m,int tri,int vnum)
{
Assert(!m.incidentTris.empty());
int v=m.tris[tri][vnum];
Vector3 n(Zero);
for(size_t i=0;i<m.incidentTris[v].size();i++)
n += m.TriangleNormal(m.incidentTris[v][i]);
n.inplaceNormalize();
return m.currentTransform.R*n;
}
int MeshPrimitiveCollide(CollisionMesh& m1,Real outerMargin1,GeometricPrimitive3D& g2,const RigidTransform& T2,Real outerMargin2,dContactGeom* contact,int maxcontacts)
{
GeometricPrimitive3D gworld=g2;
gworld.Transform(T2);
Sphere3D s;
if(gworld.type != GeometricPrimitive3D::Point && gworld.type != GeometricPrimitive3D::Sphere) {
fprintf(stderr,"Distance computations between Triangles and %s not supported\n",gworld.TypeName());
return 0;
}
if(gworld.type == GeometricPrimitive3D::Point) {
s.center = *AnyCast<Point3D>(&gworld.data);
s.radius = 0;
}
else {
s = *AnyCast<Sphere3D>(&gworld.data);
}
Real tol = outerMargin1 + outerMargin2;
Triangle3D tri;
vector<int> tris;
int k=0;
NearbyTriangles(m1,gworld,tol,tris,maxcontacts);
for(size_t j=0;j<tris.size();j++) {
m1.GetTriangle(tris[j],tri);
tri.a = m1.currentTransform*tri.a;
tri.b = m1.currentTransform*tri.b;
tri.c = m1.currentTransform*tri.c;
Vector3 cp = tri.closestPoint(s.center);
Vector3 n = cp - s.center;
Real nlen = n.length();
Real d = nlen-s.radius;
Vector3 pw = s.center;
if(s.radius > 0)
//adjust pw to the sphere surface
pw += n*(s.radius/nlen);
if(d < gNormalFromGeometryTolerance) { //compute normal from the geometry
Vector3 plocal;
m1.currentTransform.mulInverse(cp,plocal);
n = ContactNormal(m1,plocal,tris[j],pw);
}
else if(d > tol) { //some penetration -- we can't trust the result of PQP
continue;
}
else n /= nlen;
//migrate the contact point to the center of the overlap region
CopyVector(contact[k].pos,0.5*(cp+pw) + ((outerMargin2 - outerMargin1)*0.5)*n);
CopyVector(contact[k].normal,n);
contact[k].depth = tol - d;
k++;
if(k == maxcontacts) break;
}
return k;
}
bool Octree::IsCornerInsidePartition(const CollisionMesh& object, const Partition& partition) const
{
const std::vector<D3DXVECTOR3>& oabb = object.GetOABB();
for(const D3DXVECTOR3& point : oabb)
{
if(IsPointInsidePartition(point, partition))
{
return true;
}
}
return false;
}
D3DXVECTOR3 CollisionSolver::GetConvexHullPenetration(const CollisionMesh& particle,
const CollisionMesh& hull,
Simplex& simplex)
{
D3DXVECTOR3 furthestPoint;
D3DXVECTOR3 penetrationDirection;
float penetrationDistance = 0.0f;
bool penetrationFound = false;
const float minDistance = 0.1f;
const int maxIterations = 10;
int iteration = 0;
while(!penetrationFound && iteration < maxIterations)
{
++iteration;
const Face& face = simplex.GetClosestFaceToOrigin();
penetrationDirection = face.normal;
penetrationDistance = face.distanceToOrigin;
penetrationFound = penetrationDistance == 0.0f;
if(!penetrationFound)
{
// Check if there are any edge points beyond the closest face
furthestPoint = GetMinkowskiSumEdgePoint(face.normal, particle, hull);
const D3DXVECTOR3 faceToPoint = furthestPoint - simplex.GetPoint(face.indices[0]);
const float distance = fabs(D3DXVec3Dot(&faceToPoint, &face.normal));
penetrationFound = distance < minDistance;
if(!penetrationFound)
{
// Add the new point and extend the convex hull
simplex.ExtendFace(furthestPoint);
}
}
}
if(!penetrationFound)
{
// Fallback on the initial closest face
const Face& face = simplex.GetClosestFaceToOrigin();
penetrationDirection = face.normal;
penetrationDistance = face.distanceToOrigin;
}
if(particle.RenderSolverDiagnostics())
{
UpdateDiagnostics(simplex, furthestPoint);
}
return -(penetrationDirection * penetrationDistance);
}
Vector3 VertexNormal(const CollisionMesh& m,int tri,int vnum)
{
if(m.incidentTris.empty()) {
fprintf(stderr,"VertexNormal: mesh is not properly initialized with incidentTris array?\n");
return Vector3(0.0);
FatalError("VertexNormal: mesh is not properly initialized with incidentTris array?");
}
Assert(vnum >= 0 && vnum < 3);
int v=m.tris[tri][vnum];
Assert(v >= 0 && v < m.incidentTris.size());
if(m.incidentTris[v].empty()) return Vector3(0.0);
Vector3 n(Zero);
for(size_t i=0;i<m.incidentTris[v].size();i++)
n += m.TriangleNormal(m.incidentTris[v][i]);
n.inplaceNormalize();
return m.currentTransform.R*n;
}
void CollisionSolver::SolveParticleSphereCollision(CollisionMesh& particle,
const CollisionMesh& sphere)
{
D3DXVECTOR3 sphereToParticle = particle.GetPosition() - sphere.GetPosition();
const float lengthSqr = D3DXVec3LengthSq(&sphereToParticle);
const float combinedRadius = sphere.GetRadius() + particle.GetRadius();
if (lengthSqr < (combinedRadius*combinedRadius))
{
const float length = std::sqrt(lengthSqr);
sphereToParticle /= length;
particle.ResolveCollision(sphereToParticle * fabs(combinedRadius-length),
sphere.GetVelocity(), sphere.GetShape());
}
}
void CollisionSolver::SolveParticleCollision(CollisionMesh& particleA,
CollisionMesh& particleB)
{
D3DXVECTOR3 particleToParticle = particleB.GetPosition() - particleA.GetPosition();
const float lengthSqr = D3DXVec3LengthSq(&particleToParticle);
const float combinedRadius = particleA.GetRadius() + particleB.GetRadius();
if (lengthSqr < (combinedRadius*combinedRadius))
{
const float length = std::sqrt(lengthSqr);
particleToParticle /= std::sqrt(length);
const D3DXVECTOR3 translation = particleToParticle*fabs(combinedRadius-length);
particleA.ResolveCollision(-translation);
particleB.ResolveCollision(translation);
}
}
void CollisionSolver::SolveParticleHullCollision(CollisionMesh& particle,
const CollisionMesh& hull)
{
// Determine if within a rough radius of the convex hull
const D3DXVECTOR3 sphereToParticle = particle.GetPosition() - hull.GetPosition();
const float lengthSqr = D3DXVec3LengthSq(&sphereToParticle);
const float extendedParticleRadius = particle.GetRadius() * 2.0f;
const float combinedRadius = hull.GetRadius() + extendedParticleRadius;
if (lengthSqr < (combinedRadius*combinedRadius))
{
Simplex simplex;
if(AreConvexHullsColliding(particle, hull, simplex))
{
simplex.GenerateFaces();
const D3DXVECTOR3 penetration = GetConvexHullPenetration(particle, hull, simplex);
particle.ResolveCollision(penetration, hull.GetVelocity(), hull.GetShape());
}
}
}
int MeshPointCloudCollide(CollisionMesh& m1,Real outerMargin1,CollisionPointCloud& pc2,Real outerMargin2,dContactGeom* contact,int maxcontacts)
{
Real tol = outerMargin1 + outerMargin2;
int k=0;
vector<int> tris;
Triangle3D tri,triw;
for(size_t i=0;i<pc2.points.size();i++) {
Vector3 pw = pc2.currentTransform*pc2.points[i];
NearbyTriangles(m1,pw,tol,tris,maxcontacts-k);
for(size_t j=0;j<tris.size();j++) {
m1.GetTriangle(tris[j],tri);
triw.a = m1.currentTransform*tri.a;
triw.b = m1.currentTransform*tri.b;
triw.c = m1.currentTransform*tri.c;
Vector3 cp = triw.closestPoint(pw);
Vector3 n = cp - pw;
Real d = n.length();
if(d < gNormalFromGeometryTolerance) { //compute normal from the geometry
Vector3 plocal;
m1.currentTransform.mulInverse(cp,plocal);
n = ContactNormal(m1,plocal,tris[j],pw);
}
else if(d > tol) { //some penetration -- we can't trust the result of PQP
continue;
}
else n /= d;
//migrate the contact point to the center of the overlap region
CopyVector(contact[k].pos,0.5*(cp+pw) + ((outerMargin2 - outerMargin1)*0.5)*n);
CopyVector(contact[k].normal,n);
contact[k].depth = tol - d;
k++;
if(k == maxcontacts) break;
}
}
return k;
}
bool CollisionMesh::CheckCollisionsGJK2(CollisionMesh& otherMesh)
{
bool collision = false;
std::vector<XMFLOAT3> convexHull;
std::vector<VPCNTDesc> vertices = GetVertices();
std::vector<VPCNTDesc> otherVertices = otherMesh.GetVertices();
XMMATRIX otherWorld = otherMesh.GetWorldTransform();
XMMATRIX world = GetWorldTransform();
XMFLOAT3 origin = XMFLOAT3(0.0f, 0.0f, 0.0f);
// Pre-multiply the model's vertices so as to avoid transforming them during comparison.
for (int vertIndex = 0; vertIndex < vertices.size(); vertIndex++)
{
XMStoreFloat3(&vertices[vertIndex].Position, XMVector3Transform(XMLoadFloat3(&vertices[vertIndex].Position), world));
XMStoreFloat3(&vertices[vertIndex].Normal, XMVector3Transform(XMLoadFloat3(&vertices[vertIndex].Normal), world));
}
for (int otherVertIndex = 0; otherVertIndex < otherVertices.size(); otherVertIndex++)
{
XMStoreFloat3(&otherVertices[otherVertIndex].Position, XMVector3Transform(XMLoadFloat3(&otherVertices[otherVertIndex].Position), otherWorld));
XMStoreFloat3(&otherVertices[otherVertIndex].Normal, XMVector3Transform(XMLoadFloat3(&otherVertices[otherVertIndex].Normal), otherWorld));
}
std::vector<VPCNTDesc> supportingVertices;
for (int vertIndex = 0; vertIndex < vertices.size(); vertIndex++)
{
VPCNTDesc &firstVertex = vertices[vertIndex];
VPCNTDesc &secondVertex = vertices[(vertIndex + 1) % vertices.size()];
supportingVertices.clear();
supportingVertices = GetSupportingVertices(otherVertices, firstVertex.Normal);
for (int supportingVertexIndex = 0; supportingVertexIndex < supportingVertices.size(); supportingVertexIndex++)
{
XMFLOAT3 firstFormPoint, secondFormPoint;
XMStoreFloat3(&firstFormPoint, XMVectorSubtract(XMLoadFloat3(&supportingVertices[supportingVertexIndex].Position), XMLoadFloat3(&firstVertex.Position)));
XMStoreFloat3(&secondFormPoint, XMVectorSubtract(XMLoadFloat3(&supportingVertices[supportingVertexIndex].Position), XMLoadFloat3(&secondVertex.Position)));
XMFLOAT3 edgeDistValue;
XMStoreFloat3(&edgeDistValue, XMVector3Dot(XMLoadFloat3(&firstFormPoint), XMLoadFloat3(&firstVertex.Normal)));
float edgeDist = edgeDistValue.x;
// project the origin onto our edge.
XMVECTOR firstToSecondVector = XMVectorSubtract(XMLoadFloat3(&firstFormPoint), XMLoadFloat3(&secondFormPoint));
XMVECTOR secondToFirstVector = XMVectorSubtract(XMLoadFloat3(&secondFormPoint), XMLoadFloat3(&firstFormPoint));
XMVECTOR fTSDotVector = XMVector3Dot(firstToSecondVector, XMVectorSet(firstFormPoint.x - origin.x, firstFormPoint.y - origin.y, firstFormPoint.z - origin.z, 0.0f));
XMVECTOR sTFDotVector = XMVector3Dot(secondToFirstVector, XMVectorSet(secondFormPoint.x - origin.x, secondFormPoint.y - origin.y, secondFormPoint.z - origin.z, 0.0f));
XMFLOAT3 firstDotValue, secondDotValue;
XMStoreFloat3(&firstDotValue, fTSDotVector);
XMStoreFloat3(&secondDotValue, sTFDotVector);
XMFLOAT3 projectedPoint;
if (firstDotValue.x > XMConvertToRadians(90.0f))
{
projectedPoint = firstFormPoint;
}
else if (secondDotValue.x > XMConvertToRadians(90.0f))
{
projectedPoint = secondFormPoint;
}
else
{
}
}
}
return collision;
}
bool CollisionMesh::CheckCollisionsCustom(CollisionMesh &otherMesh)
{
bool collision = false;
std::vector<XMFLOAT3> convexHull;
std::vector<VPCNTDesc> vertices = GetVertices();
std::vector<VPCNTDesc> otherVertices = otherMesh.GetVertices();
XMMATRIX otherWorld = otherMesh.GetWorldTransform();
XMMATRIX world = GetWorldTransform();
XMFLOAT3 origin = XMFLOAT3(0.0f, 0.0f, 0.0f);
// Create a vector to ease the inversion calculation (we want the opposite direction for the translation vector).
XMVECTOR inverse = XMVectorSet(-1.0f, -1.0f, -1.0f, 0.0f);
XMVECTOR ourOriginDisplacement = XMVector3Transform(XMVectorSet(origin.x, origin.y, origin.z, 0.0f), world);
XMMATRIX ourOriginTransform = XMMatrixTranslationFromVector(XMVectorMultiply(ourOriginDisplacement, inverse));
// This is used for the purposes of moving the normals of the other object back to around (0, 0, 0).
XMVECTOR theirOriginDisplacement = XMVector3Transform(XMVectorSet(origin.x, origin.y, origin.z, 0.0f), otherWorld);
XMMATRIX theirOriginTransform = XMMatrixTranslationFromVector(XMVectorMultiply(theirOriginDisplacement, inverse));
XMMATRIX ourOriginTranslatedWorld = world * ourOriginTransform;
XMMATRIX theirOriginTranslatedWorld = otherWorld * ourOriginTransform;
XMMATRIX theirOriginTranslatedWorldNormalAdjustment = theirOriginTransform * otherWorld;
// Pre-multiply the model's vertices so as to avoid transforming them during comparison.
for (int vertIndex = 0; vertIndex < vertices.size(); vertIndex++)
{
XMStoreFloat3(&vertices[vertIndex].Position, XMVector3Transform(XMLoadFloat3(&vertices[vertIndex].Position), ourOriginTranslatedWorld));
XMStoreFloat3(&vertices[vertIndex].Normal, XMVector3Transform(XMLoadFloat3(&vertices[vertIndex].Normal), ourOriginTranslatedWorld));
}
for (int otherVertIndex = 0; otherVertIndex < otherVertices.size(); otherVertIndex++)
{
XMStoreFloat3(&otherVertices[otherVertIndex].Position, XMVector3Transform(XMLoadFloat3(&otherVertices[otherVertIndex].Position), theirOriginTranslatedWorld));
XMStoreFloat3(&otherVertices[otherVertIndex].Normal, XMVector3Transform(XMLoadFloat3(&otherVertices[otherVertIndex].Normal), theirOriginTranslatedWorldNormalAdjustment));
}
int potentialCollisions = 0;
std::vector<XMFLOAT3> positions;
// Now that the pre-multiplication is done, time to do our first-case checking: are we inside of it?
for (int vertIndex = 0; vertIndex < vertices.size(); vertIndex++)
{
bool localCollision = true;
XMVECTOR ourVertex = XMLoadFloat3(&vertices[vertIndex].Position);
XMVECTOR ourNormal = XMLoadFloat3(&vertices[vertIndex].Normal);
// For each vertex in our mesh, we'll check to see if it resides inside our other mesh.
for (int otherVertIndex = 0; otherVertIndex < otherVertices.size(); otherVertIndex++)
{
XMVECTOR otherVertex = XMLoadFloat3(&otherVertices[otherVertIndex].Position);
XMVECTOR otherNormal = XMLoadFloat3(&otherVertices[otherVertIndex].Normal);
XMVECTOR difference = XMVectorSubtract(ourVertex, otherVertex);
XMFLOAT3 differenceDotValue, normalDotValue;
XMVECTOR diffLength = XMVector3Length(difference);
XMVECTOR normLength = XMVector3Length(otherNormal);
XMVECTOR magnitude = XMVectorMultiply(diffLength, normLength);
XMStoreFloat3(&differenceDotValue, XMVectorDivide(XMVector3Dot(difference, otherNormal), magnitude));
// At this point, we should have the cosine of the angle.
float angleInRads = acosf(differenceDotValue.x);
float angleInDegs = XMConvertToDegrees(angleInRads);
XMStoreFloat3(&normalDotValue, XMVector3Dot(ourNormal, otherNormal));
if (angleInDegs < 90.0f)
{
localCollision = false;
}
}
if (localCollision)
{
positions.push_back(vertices[vertIndex].Position);
}
}
if (positions.empty())
{
// Time to do our second-case checking: is it inside of us?
for (int otherVertIndex = 0; otherVertIndex < otherVertices.size(); otherVertIndex++)
{
bool localCollision = true;
XMVECTOR otherVertex = XMLoadFloat3(&otherVertices[otherVertIndex].Position);
XMVECTOR otherNormal = XMVector3Normalize(XMLoadFloat3(&otherVertices[otherVertIndex].Normal));
// For each vertex in our mesh, we'll check to see if it resides inside our other mesh.
for (int vertIndex = 0; vertIndex < vertices.size(); vertIndex++)
{
XMVECTOR ourVertex = XMLoadFloat3(&vertices[vertIndex].Position);
XMVECTOR ourNormal = XMVector3Normalize(XMLoadFloat3(&vertices[vertIndex].Normal));
XMVECTOR difference = XMVectorSubtract(otherVertex, ourVertex);
XMFLOAT3 differenceDotValue, normalDotValue;
XMVECTOR diffLength = XMVector3Length(difference);
XMVECTOR normLength = XMVector3Length(ourNormal);
XMVECTOR magnitude = XMVectorMultiply(diffLength, normLength);
XMStoreFloat3(&differenceDotValue, XMVectorDivide(XMVector3Dot(difference, ourNormal), magnitude));
// At this point, we should have the cosine of the angle.
float angleInRads = acosf(differenceDotValue.x);
float angleInDegs = XMConvertToDegrees(angleInRads);
XMStoreFloat3(&normalDotValue, XMVector3Dot(ourNormal, otherNormal));
if (angleInDegs < 90.0f)
{
localCollision = false;
}
}
if (localCollision)
{
positions.push_back(otherVertices[otherVertIndex].Position);
}
}
}
if(positions.size())
{
mDelegate->CollisionOccurred(otherMesh.mDelegate);
otherMesh.mDelegate->CollisionOccurred(mDelegate);
}
return positions.size();
}
int MeshPointCloudCollide(CollisionMesh& m1,Real outerMargin1,CollisionPointCloud& pc2,Real outerMargin2,dContactGeom* contact,int maxcontacts)
{
Real tol=outerMargin1+outerMargin2;
vector<int> points;
vector<int> tris;
if(!Collides(pc2,tol,m1,points,tris,maxcontacts)) return 0;
Assert(points.size()==tris.size());
Triangle3D tri,triw;
int k=0;
for(size_t i=0;i<points.size();i++) {
Vector3 pw = pc2.currentTransform*pc2.points[points[i]];
m1.GetTriangle(tris[i],tri);
triw.a = m1.currentTransform*tri.a;
triw.b = m1.currentTransform*tri.b;
triw.c = m1.currentTransform*tri.c;
Vector3 cp = triw.closestPoint(pw);
Vector3 n = cp - pw;
Real d = n.length();
if(d < gNormalFromGeometryTolerance) { //compute normal from the geometry
Vector3 plocal;
m1.currentTransform.mulInverse(cp,plocal);
n = ContactNormal(m1,plocal,tris[i],pw);
}
else if(d > tol) { //some penetration -- we can't trust the result of PQP
continue;
}
else n /= d;
//migrate the contact point to the center of the overlap region
CopyVector(contact[k].pos,0.5*(cp+pw) + ((outerMargin2 - outerMargin1)*0.5)*n);
CopyVector(contact[k].normal,n);
contact[k].depth = tol - d;
k++;
if(k == maxcontacts) break;
}
/*
Real tol = outerMargin1 + outerMargin2;
Box3D mbb,mbb_pclocal;
GetBB(m1,mbb);
RigidTransform Tw_pc;
Tw_pc.setInverse(pc2.currentTransform);
mbb_pclocal.setTransformed(mbb,Tw_pc);
AABB3D maabb_pclocal;
mbb_pclocal.getAABB(maabb_pclocal);
maabb_pclocal.bmin -= Vector3(tol);
maabb_pclocal.bmax += Vector3(tol);
maabb_pclocal.setIntersection(pc2.bblocal);
list<void*> nearpoints;
pc2.grid.BoxItems(Vector(3,maabb_pclocal.bmin),Vector(3,maabb_pclocal.bmax),nearpoints);
int k=0;
vector<int> tris;
Triangle3D tri,triw;
for(list<void*>::iterator i=nearpoints.begin();i!=nearpoints.end();i++) {
Vector3 pcpt = *reinterpret_cast<Vector3*>(*i);
Vector3 pw = pc2.currentTransform*pcpt;
NearbyTriangles(m1,pw,tol,tris,maxcontacts-k);
for(size_t j=0;j<tris.size();j++) {
m1.GetTriangle(tris[j],tri);
triw.a = m1.currentTransform*tri.a;
triw.b = m1.currentTransform*tri.b;
triw.c = m1.currentTransform*tri.c;
Vector3 cp = triw.closestPoint(pw);
Vector3 n = cp - pw;
Real d = n.length();
if(d < gNormalFromGeometryTolerance) { //compute normal from the geometry
Vector3 plocal;
m1.currentTransform.mulInverse(cp,plocal);
n = ContactNormal(m1,plocal,tris[j],pw);
}
else if(d > tol) { //some penetration -- we can't trust the result of PQP
continue;
}
else n /= d;
//migrate the contact point to the center of the overlap region
CopyVector(contact[k].pos,0.5*(cp+pw) + ((outerMargin2 - outerMargin1)*0.5)*n);
CopyVector(contact[k].normal,n);
contact[k].depth = tol - d;
k++;
if(k == maxcontacts) break;
}
}
return k;
*/
return k;
}
int MeshMeshCollide(CollisionMesh& m1,Real outerMargin1,CollisionMesh& m2,Real outerMargin2,dContactGeom* contact,int maxcontacts)
{
CollisionMeshQuery q(m1,m2);
bool res=q.WithinDistanceAll(outerMargin1+outerMargin2);
if(!res) {
return 0;
}
vector<int> t1,t2;
vector<Vector3> cp1,cp2;
q.TolerancePairs(t1,t2);
q.TolerancePoints(cp1,cp2);
//printf("%d Collision pairs\n",t1.size());
const RigidTransform& T1 = m1.currentTransform;
const RigidTransform& T2 = m2.currentTransform;
RigidTransform T21; T21.mulInverseA(T1,T2);
RigidTransform T12; T12.mulInverseA(T2,T1);
Real tol = outerMargin1+outerMargin2;
Real tol2 = Sqr(tol);
size_t imax=t1.size();
Triangle3D tri1,tri2,tri1loc,tri2loc;
if(gDoTriangleTriangleCollisionDetection) {
//test if more triangle vertices are closer than tolerance
for(size_t i=0;i<imax;i++) {
m1.GetTriangle(t1[i],tri1);
m2.GetTriangle(t2[i],tri2);
tri1loc.a = T12*tri1.a;
tri1loc.b = T12*tri1.b;
tri1loc.c = T12*tri1.c;
tri2loc.a = T21*tri2.a;
tri2loc.b = T21*tri2.b;
tri2loc.c = T21*tri2.c;
bool usecpa,usecpb,usecpc,usecpa2,usecpb2,usecpc2;
Vector3 cpa = tri1.closestPoint(tri2loc.a);
Vector3 cpb = tri1.closestPoint(tri2loc.b);
Vector3 cpc = tri1.closestPoint(tri2loc.c);
Vector3 cpa2 = tri2.closestPoint(tri1loc.a);
Vector3 cpb2 = tri2.closestPoint(tri1loc.b);
Vector3 cpc2 = tri2.closestPoint(tri1loc.c);
usecpa = (cpa.distanceSquared(tri2loc.a) < tol2);
usecpb = (cpb.distanceSquared(tri2loc.b) < tol2);
usecpc = (cpc.distanceSquared(tri2loc.c) < tol2);
usecpa2 = (cpa2.distanceSquared(tri1loc.a) < tol2);
usecpb2 = (cpb2.distanceSquared(tri1loc.b) < tol2);
usecpc2 = (cpc2.distanceSquared(tri1loc.c) < tol2);
//if already existing, disable it
if(usecpa && cpa.isEqual(cp1[i],cptol)) usecpa=false;
if(usecpb && cpb.isEqual(cp1[i],cptol)) usecpb=false;
if(usecpc && cpc.isEqual(cp1[i],cptol)) usecpc=false;
if(usecpa2 && cpa2.isEqual(cp2[i],cptol)) usecpa2=false;
if(usecpb2 && cpb2.isEqual(cp2[i],cptol)) usecpb2=false;
if(usecpc2 && cpc2.isEqual(cp2[i],cptol)) usecpc2=false;
if(usecpa) {
if(usecpb && cpb.isEqual(cpa,cptol)) usecpb=false;
if(usecpc && cpc.isEqual(cpa,cptol)) usecpc=false;
}
if(usecpb) {
if(usecpc && cpc.isEqual(cpb,cptol)) usecpc=false;
}
if(usecpa2) {
if(usecpb2 && cpb2.isEqual(cpa2,cptol)) usecpb2=false;
if(usecpc2 && cpc2.isEqual(cpa2,cptol)) usecpc2=false;
}
if(usecpb) {
if(usecpc2 && cpc.isEqual(cpb2,cptol)) usecpc2=false;
}
if(usecpa) {
t1.push_back(t1[i]);
t2.push_back(t2[i]);
cp1.push_back(cpa);
cp2.push_back(tri2.a);
}
if(usecpb) {
t1.push_back(t1[i]);
t2.push_back(t2[i]);
cp1.push_back(cpb);
cp2.push_back(tri2.b);
}
if(usecpc) {
t1.push_back(t1[i]);
t2.push_back(t2[i]);
cp1.push_back(cpc);
cp2.push_back(tri2.c);
}
if(usecpa2) {
t1.push_back(t1[i]);
t2.push_back(t2[i]);
cp1.push_back(tri1.a);
cp2.push_back(cpa2);
}
if(usecpb2) {
t1.push_back(t1[i]);
t2.push_back(t2[i]);
cp1.push_back(tri1.b);
cp2.push_back(cpb2);
}
if(usecpc2) {
t1.push_back(t1[i]);
t2.push_back(t2[i]);
cp1.push_back(tri1.c);
cp2.push_back(cpc2);
}
}
/*
if(t1.size() != imax)
printf("ODECustomMesh: Triangle vert checking added %d points\n",t1.size()-imax);
*/
//getchar();
}
imax = t1.size();
static int warnedCount = 0;
for(size_t i=0;i<imax;i++) {
m1.GetTriangle(t1[i],tri1);
m2.GetTriangle(t2[i],tri2);
tri1loc.a = T12*tri1.a;
tri1loc.b = T12*tri1.b;
tri1loc.c = T12*tri1.c;
if(tri1loc.intersects(tri2)) {
if(warnedCount % 1000 == 0) {
printf("ODECustomMesh: Triangles penetrate margin %g+%g: can't trust contact detector\n",outerMargin1,outerMargin2);
}
warnedCount++;
/*
//the two triangles intersect! can't trust results of PQP
t1[i] = t1.back();
t2[i] = t2.back();
cp1[i] = cp1.back();
cp2[i] = cp2.back();
i--;
imax--;
*/
}
}
if(t1.size() != imax) {
printf("ODECustomMesh: %d candidate points were removed due to mesh collision\n",t1.size()-imax);
t1.resize(imax);
t2.resize(imax);
cp1.resize(imax);
cp2.resize(imax);
}
int k=0; //count the # of contact points added
for(size_t i=0;i<cp1.size();i++) {
Vector3 p1 = T1*cp1[i];
Vector3 p2 = T2*cp2[i];
Vector3 n=p1-p2;
Real d = n.norm();
if(d < gNormalFromGeometryTolerance) { //compute normal from the geometry
n = ContactNormal(m1,m2,cp1[i],cp2[i],t1[i],t2[i]);
}
else if(d > tol) { //some penetration -- we can't trust the result of PQP
continue;
}
else n /= d;
//check for invalid normals
Real len=n.length();
if(len < gZeroNormalTolerance || !IsFinite(len)) continue;
//cout<<"Local Points "<<cp1[i]<<", "<<cp2[i]<<endl;
//cout<<"Points "<<p1<<", "<<p2<<endl;
//Real utol = (tol)*0.5/d + 0.5;
//CopyVector(contact[k].pos,p1+utol*(p2-p1));
CopyVector(contact[k].pos,0.5*(p1+p2) + ((outerMargin2 - outerMargin1)*0.5)*n);
CopyVector(contact[k].normal,n);
contact[k].depth = tol - d;
if(contact[k].depth < 0) contact[k].depth = 0;
//cout<<"Normal "<<n<<", depth "<<contact[i].depth<<endl;
//getchar();
k++;
if(k == maxcontacts) break;
}
return k;
}
void Octree::RemoveObject(CollisionMesh& object)
{
object.GetPartition()->RemoveNode(object);
object.SetPartition(nullptr);
}
///Compute normal from mesh geometry: returns the local normal needed for
///triangle 1 on m1 to get out of triangle 2 on m2.
///p1 and p2 are given in local coordinates
Vector3 ContactNormal(const CollisionMesh& m1,const CollisionMesh& m2,const Vector3& p1,const Vector3& p2,int t1,int t2)
{
Triangle3D tri1,tri2;
m1.GetTriangle(t1,tri1);
m2.GetTriangle(t2,tri2);
Vector3 b1=tri1.barycentricCoords(p1);
Vector3 b2=tri2.barycentricCoords(p2);
int type1=FeatureType(b1),type2=FeatureType(b2);
switch(type1) {
case 1: //pt
switch(type2) {
case 1: //pt
//get the triangle normals
{
//printf("ODECustomMesh: Point-point contact\n");
Vector3 n1 = VertexNormal(m1,t1,VertexIndex(b1));
Vector3 n2 = VertexNormal(m2,t2,VertexIndex(b2));
n2 -= n1;
n2.inplaceNormalize();
return n2;
}
break;
case 2: //edge
{
//printf("ODECustomMesh: Point-edge contact\n");
Vector3 n1 = VertexNormal(m1,t1,VertexIndex(b1));
int e = EdgeIndex(b2);
Segment3D s = tri2.edge(e);
Vector3 ev = m2.currentTransform.R*(s.b-s.a);
Vector3 n2 = EdgeNormal(m2,t2,e);
n2-=(n1-ev*ev.dot(n1)/ev.dot(ev)); //project onto normal
n2.inplaceNormalize();
return n2;
}
break;
case 3: //face
return m2.currentTransform.R*tri2.normal();
}
break;
case 2: //edge
switch(type2) {
case 1: //pt
{
//printf("ODECustomMesh: Edge-point contact\n");
Vector3 n2 = VertexNormal(m2,t2,VertexIndex(b2));
int e = EdgeIndex(b1);
Segment3D s = tri1.edge(e);
Vector3 ev = m1.currentTransform.R*(s.b-s.a);
Vector3 n1 = EdgeNormal(m1,t1,e);
n2 = (n2-ev*ev.dot(n2)/ev.dot(ev))-n1; //project onto normal
n2.inplaceNormalize();
return n2;
}
break;
case 2: //edge
{
//printf("ODECustomMesh: Edge-edge contact\n");
int e = EdgeIndex(b1);
Segment3D s1 = tri1.edge(e);
Vector3 ev1 = m1.currentTransform.R*(s1.b-s1.a);
ev1.inplaceNormalize();
e = EdgeIndex(b2);
Segment3D s2 = tri2.edge(e);
Vector3 ev2 = m2.currentTransform.R*(s2.b-s2.a);
ev2.inplaceNormalize();
Vector3 n;
n.setCross(ev1,ev2);
Real len = n.length();
if(len < gZeroNormalTolerance) {
//hmm... edges are parallel?
}
n /= len;
//make sure the normal direction points into m1 and out of m2
if(n.dot(m1.currentTransform*s1.a) < n.dot(m2.currentTransform*s2.a))
n.inplaceNegative();
/*
if(n.dot(m1.currentTransform.R*tri1.normal()) > 0.0) {
if(n.dot(m2.currentTransform.R*tri2.normal()) > 0.0) {
printf("ODECustomMesh: Warning, inconsistent normal direction? %g, %g\n",n.dot(m1.currentTransform.R*tri1.normal()),n.dot(m2.currentTransform.R*tri2.normal()));
}
n.inplaceNegative();
}
else {
if(n.dot(m2.currentTransform.R*tri2.normal()) < 0.0) {
printf("ODECustomMesh: Warning, inconsistent normal direction? %g, %g\n",n.dot(m1.currentTransform.R*tri1.normal()),n.dot(m2.currentTransform.R*tri2.normal()));
}
}
*/
//cout<<"Edge vector 1 "<<ev1<<", vector 2" <<ev2<<", normal: "<<n<<endl;
return n;
}
break;
case 3: //face
return m2.currentTransform.R*tri2.normal();
}
break;
case 3: //face
if(type2 == 3)
printf("ODECustomMesh: Warning, face-face contact?\n");
return m1.currentTransform.R*(-tri1.normal());
}
static int warnedCount = 0;
if(warnedCount % 10000 == 0)
printf("ODECustomMesh: Warning, degenerate triangle, types %d %d\n",type1,type2);
warnedCount++;
//AssertNotReached();
return Vector3(Zero);
}
LevelObject * LevelObject::CreateCube()
{
EngineMesh * mesh = new EngineMesh();
{
shared_ptr<PackContentHeader> pch = PackList::SharedInstance()->FindObject("SCMarineMeshFull");
if(pch)
{
char* buffer = (char*)malloc(pch->_size);
pch->_pack->ReadObjectFromPack(buffer, pch);
mesh->LoadFromBuffer(buffer, pch->_size);
free(buffer);
}
}
TextureMaterial *material = new TextureMaterial();
Texture * texture = new Texture();
{
shared_ptr<PackContentHeader> pch = PackList::SharedInstance()->FindObject("SCMarineTextureDiffuse");
if(pch)
{
char* buffer = (char*)malloc(pch->_size);
pch->_pack->ReadObjectFromPack(buffer, pch);
texture->LoadFromBuffer(buffer, pch->_size);
free(buffer);
material->SetTexture(texture);
}
}
btCollisionShape *shape = NULL;
{
shared_ptr<PackContentHeader> pch = PackList::SharedInstance()->FindObject("WoodenCrate10CollisionMesh");
if(pch)
{
CollisionMesh* cm = new CollisionMesh();
char* buffer = (char*)malloc(pch->_size);
pch->_pack->ReadObjectFromPack(buffer, pch);
cm->LoadFromBuffer(buffer, pch->_size);
free(buffer);
shape = btCollisionShapeFromCollisionMesh(cm);
delete cm;
}
if (!shape)
shape = new btBoxShape(btVector3(0.5, 0.5, 0.5));
}
btVector3 fallInertia(0,0,0);
shape->calculateLocalInertia(10, fallInertia);
btDefaultMotionState *motionState = new btDefaultMotionState();
btTransform t;
t.setRotation(btQuaternion(btVector3(1, 0, 0), 0));
motionState->setWorldTransform(t);
btRigidBody::btRigidBodyConstructionInfo info(10, motionState, shape, fallInertia);
btRigidBody *body = new btRigidBody(info);
body->setCollisionFlags(0);
body->setRestitution(0.01);
ObjectBehaviourModel *obmm = new PhysicObjectBehaviuorModel(body);
RenderObject *renderObject = new UnAnimRenderObject(mesh);
LevelObject * result = new LevelObject(renderObject, obmm, material);
return result;
}
void CollisionMesh::LoadInstance(const CollisionMesh& mesh)
{
m_geometry = mesh.GetGeometry();
Initialise(false, m_geometry->GetShape(),
mesh.m_minLocalScale, mesh.m_maxLocalScale);
}
bool Segment::Intersects( const CollisionMesh& m, CollisionInfo* const pInfo /*= NULL*/ ) const
{
return m.Intersects( *this, pInfo );
}
bool CollisionMesh::CheckCollisionsGJK1(CollisionMesh& otherMesh)
{
std::vector<XMFLOAT3> convexHull;
bool foundOrigin = false;
std::vector<VPCNTDesc> vertices = GetVertices();
std::vector<VPCNTDesc> otherVertices = otherMesh.GetVertices();
XMMATRIX otherWorld = otherMesh.GetWorldTransform();
XMMATRIX world = GetWorldTransform();
// Pre-multiply the model's vertices so as to avoid transforming them during comparison.
for (int vertIndex = 0; vertIndex < vertices.size(); vertIndex++)
{
XMVECTOR vertexTransform = XMLoadFloat3(&vertices[vertIndex].Position);
XMStoreFloat3(&vertices[vertIndex].Position, XMVector3Transform(vertexTransform, world));
}
for (int otherVertIndex = 0; otherVertIndex < otherVertices.size(); otherVertIndex++)
{
XMVECTOR vertexTransform = XMLoadFloat3(&otherVertices[otherVertIndex].Position);
XMStoreFloat3(&otherVertices[otherVertIndex].Position, XMVector3Transform(vertexTransform, otherWorld));
}
// Now we get to the fun part; the subtraction.
for (int vertIndex = 0; vertIndex < vertices.size() && !foundOrigin; vertIndex++)
{
XMFLOAT3 vertexValue = vertices[vertIndex].Position;
XMVECTOR vertexTransform = XMLoadFloat3(&vertexValue);
for (int otherVertIndex = 0; otherVertIndex < otherVertices.size() && !foundOrigin; otherVertIndex++)
{
XMVECTOR otherVertexTransform = XMLoadFloat3(&otherVertices[otherVertIndex].Position);
XMFLOAT3 convexHullPoint;
XMVECTOR difference = XMVectorSubtract(vertexTransform, otherVertexTransform);
XMStoreFloat3(&convexHullPoint, difference);
convexHull.push_back(convexHullPoint);
foundOrigin = XMVector3Equal(difference, XMVectorZero());
}
convexHull.push_back(vertexValue);
}
if (!foundOrigin)
{
XMFLOAT3 collisionLine = XMFLOAT3(0.0f, 1250.0f, 500.0f);
printf("We ain't found shit!");
bool collision = true;
int intersections = 0;
for (int hullVertexIndex = 0; hullVertexIndex < convexHull.size() && convexHull.size() > 3; hullVertexIndex += 3)
{
int secondIndex = (hullVertexIndex + 1) % (convexHull.size() - 1);
int thirdIndex = (hullVertexIndex + 2) % (convexHull.size() - 1);
XMFLOAT3 firstVert = convexHull[hullVertexIndex];
XMFLOAT3 secondVert = convexHull[secondIndex];
XMFLOAT3 thirdVert = convexHull[thirdIndex];
XMFLOAT3 origin = XMFLOAT3(0.0f, 0.0f, 0.0f);
// we need to check the normal. Calculate using cross product.
XMVECTOR firstVector = XMVectorSet(secondVert.x - firstVert.x, secondVert.y - firstVert.y, secondVert.z - firstVert.z, 0.0f);
XMVECTOR secondVector = XMVectorSet(thirdVert.x - secondVert.x, thirdVert.y - secondVert.y, thirdVert.z - secondVert.z, 0.0f);
XMFLOAT3 normal;
XMStoreFloat3(&normal, XMVector3Normalize(XMVector3Cross(firstVector, secondVector)));
// check to ensure no parallels are detected.
float firstDot = (normal.x * collisionLine.x) + (normal.y * collisionLine.y) + (normal.z * collisionLine.z);
if (firstDot < 0)
{
float delta = -((normal.x * (origin.x - firstVert.x)) + (normal.y * (origin.y - firstVert.y)) + (normal.z * (origin.z - firstVert.y))) /
firstDot;
if (delta < 0)
{
break;
}
XMFLOAT3 pointToCheck = XMFLOAT3(origin.x - (collisionLine.x * delta), origin.y - (collisionLine.y * delta), origin.z * (collisionLine.z * delta));
bool firstCheck = CheckWinding(firstVert, secondVert, pointToCheck, normal);
bool secondCheck = CheckWinding(secondVert, thirdVert, pointToCheck, normal);
bool thirdCheck = CheckWinding(thirdVert, firstVert, pointToCheck, normal);
if (firstCheck && secondCheck && thirdCheck)
{
intersections++;
}
else
{
collision = false;
}
}
}
if ((intersections % 2) == 1)
{
foundOrigin = true;
}
}
return foundOrigin;
}
