Wojtek Figat
263
Source/Engine/Physics/PhysX/SimulationEventCallbackPhysX.cpp
Normal file
263
Source/Engine/Physics/PhysX/SimulationEventCallbackPhysX.cpp
Normal file
@@ -0,0 +1,263 @@
|
||||
// Copyright (c) 2012-2022 Wojciech Figat. All rights reserved.
|
||||
|
||||
#if COMPILE_WITH_PHYSX
|
||||
|
||||
#include "SimulationEventCallbackPhysX.h"
|
||||
#include "Engine/Physics/Colliders/Collider.h"
|
||||
#include "Engine/Physics/Joints/Joint.h"
|
||||
#include "Engine/Physics/Actors/RigidBody.h"
|
||||
#include <ThirdParty/PhysX/extensions/PxJoint.h>
|
||||
#include <ThirdParty/PhysX/PxShape.h>
|
||||
|
||||
namespace
|
||||
{
|
||||
void ClearColliderFromCollection(PhysicsColliderActor* collider, Array<SimulationEventCallback::CollidersPair>& collection)
|
||||
{
|
||||
for (int32 i = 0; i < collection.Count(); i++)
|
||||
{
|
||||
if (collection[i].First == collider || collection[i].Second == collider)
|
||||
{
|
||||
collection.RemoveAt(i--);
|
||||
if (collection.IsEmpty())
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void ClearColliderFromCollection(PhysicsColliderActor* collider, SimulationEventCallback::CollisionsPool& collection)
|
||||
{
|
||||
for (auto i = collection.Begin(); i.IsNotEnd(); ++i)
|
||||
{
|
||||
if (i->Key.First == collider || i->Key.Second == collider)
|
||||
{
|
||||
collection.Remove(i);
|
||||
if (collection.IsEmpty())
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void SimulationEventCallback::Clear()
|
||||
{
|
||||
PrevCollisions.Swap(Collisions);
|
||||
Collisions.Clear();
|
||||
|
||||
NewCollisions.Clear();
|
||||
RemovedCollisions.Clear();
|
||||
|
||||
NewTriggerPairs.Clear();
|
||||
LostTriggerPairs.Clear();
|
||||
|
||||
BrokenJoints.Clear();
|
||||
}
|
||||
|
||||
void SimulationEventCallback::CollectResults()
|
||||
{
|
||||
// Generate new collisions
|
||||
for (auto i = Collisions.Begin(); i.IsNotEnd(); ++i)
|
||||
{
|
||||
if (!PrevCollisions.ContainsKey(i->Key))
|
||||
NewCollisions.Add(i->Key);
|
||||
}
|
||||
|
||||
// Generate removed collisions
|
||||
for (auto i = PrevCollisions.Begin(); i.IsNotEnd(); ++i)
|
||||
{
|
||||
if (!Collisions.ContainsKey(i->Key))
|
||||
RemovedCollisions.Add(i->Key);
|
||||
}
|
||||
}
|
||||
|
||||
void SimulationEventCallback::SendCollisionEvents()
|
||||
{
|
||||
for (int32 i = 0; i < RemovedCollisions.Count(); i++)
|
||||
{
|
||||
const auto pair = RemovedCollisions[i];
|
||||
auto& c = PrevCollisions[pair];
|
||||
|
||||
pair.First->OnCollisionExit(c);
|
||||
|
||||
c.SwapObjects();
|
||||
pair.Second->OnCollisionExit(c);
|
||||
c.SwapObjects();
|
||||
}
|
||||
for (int32 i = 0; i < NewCollisions.Count(); i++)
|
||||
{
|
||||
const auto pair = NewCollisions[i];
|
||||
auto& c = Collisions[pair];
|
||||
|
||||
pair.First->OnCollisionEnter(c);
|
||||
c.SwapObjects();
|
||||
pair.Second->OnCollisionEnter(c);
|
||||
c.SwapObjects();
|
||||
}
|
||||
}
|
||||
|
||||
void SimulationEventCallback::SendTriggerEvents()
|
||||
{
|
||||
for (int32 i = 0; i < LostTriggerPairs.Count(); i++)
|
||||
{
|
||||
const auto c = LostTriggerPairs[i];
|
||||
c.First->OnTriggerExit(c.Second);
|
||||
c.Second->OnTriggerExit(c.First);
|
||||
}
|
||||
for (int32 i = 0; i < NewTriggerPairs.Count(); i++)
|
||||
{
|
||||
const auto c = NewTriggerPairs[i];
|
||||
c.First->OnTriggerEnter(c.Second);
|
||||
c.Second->OnTriggerEnter(c.First);
|
||||
}
|
||||
}
|
||||
|
||||
void SimulationEventCallback::SendJointEvents()
|
||||
{
|
||||
for (auto* actor : BrokenJoints)
|
||||
{
|
||||
actor->OnJointBreak();
|
||||
}
|
||||
}
|
||||
|
||||
void SimulationEventCallback::OnColliderRemoved(PhysicsColliderActor* collider)
|
||||
{
|
||||
ClearColliderFromCollection(collider, Collisions);
|
||||
ClearColliderFromCollection(collider, PrevCollisions);
|
||||
ClearColliderFromCollection(collider, RemovedCollisions);
|
||||
ClearColliderFromCollection(collider, NewTriggerPairs);
|
||||
ClearColliderFromCollection(collider, LostTriggerPairs);
|
||||
}
|
||||
|
||||
void SimulationEventCallback::OnJointRemoved(Joint* joint)
|
||||
{
|
||||
BrokenJoints.Remove(joint);
|
||||
}
|
||||
|
||||
void SimulationEventCallback::onConstraintBreak(PxConstraintInfo* constraints, PxU32 count)
|
||||
{
|
||||
for (uint32 i = 0; i < count; i++)
|
||||
{
|
||||
PxJoint* joint = reinterpret_cast<PxJoint*>(constraints[i].externalReference);
|
||||
if (joint->userData)
|
||||
BrokenJoints.Add(static_cast<Joint*>(joint->userData));
|
||||
}
|
||||
}
|
||||
|
||||
void SimulationEventCallback::onWake(PxActor** actors, PxU32 count)
|
||||
{
|
||||
// Not used
|
||||
}
|
||||
|
||||
void SimulationEventCallback::onSleep(PxActor** actors, PxU32 count)
|
||||
{
|
||||
// Not used
|
||||
}
|
||||
|
||||
void SimulationEventCallback::onContact(const PxContactPairHeader& pairHeader, const PxContactPair* pairs, PxU32 nbPairs)
|
||||
{
|
||||
// Skip sending events to removed actors
|
||||
if (pairHeader.flags & (PxContactPairHeaderFlag::eREMOVED_ACTOR_0 | PxContactPairHeaderFlag::eREMOVED_ACTOR_1))
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
Collision c;
|
||||
c.ThisVelocity = c.OtherVelocity = Vector3::Zero;
|
||||
|
||||
// Extract collision pairs
|
||||
for (PxU32 pairIndex = 0; pairIndex < nbPairs; pairIndex++)
|
||||
{
|
||||
const PxContactPair& pair = pairs[pairIndex];
|
||||
PxContactStreamIterator i(pair.contactPatches, pair.contactPoints, pair.getInternalFaceIndices(), pair.patchCount, pair.contactCount);
|
||||
|
||||
const PxReal* impulses = pair.contactImpulses;
|
||||
//const PxU32 flippedContacts = (pair.flags & PxContactPairFlag::eINTERNAL_CONTACTS_ARE_FLIPPED);
|
||||
const PxU32 hasImpulses = (pair.flags & PxContactPairFlag::eINTERNAL_HAS_IMPULSES);
|
||||
PxU32 nbContacts = 0;
|
||||
PxVec3 totalImpulse = PxVec3(0.0f);
|
||||
|
||||
c.ThisActor = static_cast<PhysicsColliderActor*>(pair.shapes[0]->userData);
|
||||
c.OtherActor = static_cast<PhysicsColliderActor*>(pair.shapes[1]->userData);
|
||||
ASSERT_LOW_LAYER(c.ThisActor && c.OtherActor);
|
||||
|
||||
while (i.hasNextPatch())
|
||||
{
|
||||
i.nextPatch();
|
||||
while (i.hasNextContact() && nbContacts < COLLISION_NAX_CONTACT_POINTS)
|
||||
{
|
||||
i.nextContact();
|
||||
|
||||
const PxVec3 point = i.getContactPoint();
|
||||
const PxVec3 normal = i.getContactNormal();
|
||||
if (hasImpulses)
|
||||
totalImpulse += normal * impulses[nbContacts];
|
||||
|
||||
//PxU32 internalFaceIndex0 = flippedContacts ? iter.getFaceIndex1() : iter.getFaceIndex0();
|
||||
//PxU32 internalFaceIndex1 = flippedContacts ? iter.getFaceIndex0() : iter.getFaceIndex1();
|
||||
|
||||
ContactPoint& contact = c.Contacts[nbContacts];
|
||||
contact.Point = P2C(point);
|
||||
contact.Normal = P2C(normal);
|
||||
contact.Separation = i.getSeparation();
|
||||
|
||||
nbContacts++;
|
||||
}
|
||||
}
|
||||
|
||||
c.ContactsCount = nbContacts;
|
||||
c.Impulse = P2C(totalImpulse);
|
||||
Collisions[CollidersPair(c.ThisActor, c.OtherActor)] = c;
|
||||
}
|
||||
|
||||
// Extract velocities
|
||||
PxContactPairExtraDataIterator i(pairHeader.extraDataStream, pairHeader.extraDataStreamSize);
|
||||
while (i.nextItemSet())
|
||||
{
|
||||
if (i.postSolverVelocity)
|
||||
{
|
||||
const PxVec3 linearVelocityActor0 = i.postSolverVelocity->linearVelocity[0];
|
||||
const PxVec3 linearVelocityActor1 = i.postSolverVelocity->linearVelocity[1];
|
||||
|
||||
const PxContactPair& pair = pairs[i.contactPairIndex];
|
||||
c.ThisActor = static_cast<PhysicsColliderActor*>(pair.shapes[0]->userData);
|
||||
c.OtherActor = static_cast<PhysicsColliderActor*>(pair.shapes[1]->userData);
|
||||
ASSERT_LOW_LAYER(c.ThisActor && c.OtherActor);
|
||||
Collision& collision = Collisions[CollidersPair(c.ThisActor, c.OtherActor)];
|
||||
|
||||
collision.ThisVelocity = P2C(linearVelocityActor0);
|
||||
collision.OtherVelocity = P2C(linearVelocityActor1);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void SimulationEventCallback::onTrigger(PxTriggerPair* pairs, PxU32 count)
|
||||
{
|
||||
for (PxU32 i = 0; i < count; i++)
|
||||
{
|
||||
const PxTriggerPair& pair = pairs[i];
|
||||
|
||||
// Ignore pairs when shapes have been deleted
|
||||
if (pair.flags & (PxTriggerPairFlag::eREMOVED_SHAPE_TRIGGER | PxTriggerPairFlag::eREMOVED_SHAPE_OTHER))
|
||||
continue;
|
||||
|
||||
auto trigger = static_cast<PhysicsColliderActor*>(pair.triggerShape->userData);
|
||||
auto otherCollider = static_cast<PhysicsColliderActor*>(pair.otherShape->userData);
|
||||
ASSERT_LOW_LAYER(trigger && otherCollider);
|
||||
CollidersPair collidersPair(trigger, otherCollider);
|
||||
|
||||
if (pair.status & PxPairFlag::eNOTIFY_TOUCH_LOST)
|
||||
{
|
||||
LostTriggerPairs.Add(collidersPair);
|
||||
}
|
||||
else
|
||||
{
|
||||
NewTriggerPairs.Add(collidersPair);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void SimulationEventCallback::onAdvance(const PxRigidBody* const* bodyBuffer, const PxTransform* poseBuffer, const PxU32 count)
|
||||
{
|
||||
// Not used
|
||||
}
|
||||
|
||||
#endif
|
||||
Reference in New Issue
Block a user