C++ CollisionMesh类代码示例

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;
}