public unsafe void Execute()
{
// Add a contact to any dynamic spheres
for (int m = 0; m < Motions.Length; m++)
{
if (Bodies[m].Collider->Type == ColliderType.Sphere)
{
var sc = (SphereCollider *)Bodies[m].Collider;
float3 bodyPos = Bodies[m].WorldFromBody.pos;
var ch = new ContactHeader();
ch.BodyPair.BodyAIndex = m;
ch.BodyPair.BodyBIndex = Motions.Length;
ch.NumContacts = 1;
ch.Normal = new float3(0, 1, 0);
var cp = new ContactPoint
{
Distance = math.dot(ch.Normal, bodyPos) - sc->Radius,
Position = bodyPos - ch.Normal * math.dot(ch.Normal, bodyPos)
};
Contacts.AddContact(ch, cp);
}
}
Contacts.CommitAddedContacts();
}
protected override JobHandle OnUpdate(JobHandle inputDeps)
{
if (m_ContactModifierGroup.CalculateLength() == 0)
{
return(inputDeps);
}
if (m_StepPhysicsWorld.Simulation.Type == SimulationType.NoPhysics)
{
return(inputDeps);
}
var modifiers = m_ContactModifierGroup.ToComponentDataArray <ModifyNarrowphaseContacts>(Allocator.TempJob);
var surfaceNormal = modifiers[0].surfaceNormal;
var surfaceRBIdx = m_BuildPhysicsWorld.PhysicsWorld.GetRigidBodyIndex(modifiers[0].surfaceEntity);
SimulationCallbacks.Callback callback = (ref ISimulation simulation, JobHandle inDeps) =>
{
inDeps.Complete(); // TODO: shouldn't be needed (jobify the below)
SimulationData.Contacts.Iterator iterator = simulation.Contacts.GetIterator();
while (iterator.HasItemsLeft())
{
ContactHeader manifold = iterator.GetNextContactHeader();
bool bUpdateNormal = (manifold.BodyPair.BodyAIndex == surfaceRBIdx) || (manifold.BodyPair.BodyBIndex == surfaceRBIdx);
float distanceScale = 1;
if (bUpdateNormal)
{
var newNormal = surfaceNormal;
distanceScale = math.dot(newNormal, manifold.Normal);
//<todo.eoin.hpi Feels pretty weird.
//<todo.eoin.hp Need to make this work if user has read a contact
iterator.SetManifoldNormal(newNormal);
}
for (int i = 0; i < manifold.NumContacts; i++)
{
ContactPoint cp = iterator.GetNextContact();
if (bUpdateNormal)
{
cp.Distance *= distanceScale;
iterator.UpdatePreviousContact(cp);
}
}
}
return(inDeps);
};
modifiers.Dispose();
m_StepPhysicsWorld.EnqueueCallback(SimulationCallbacks.Phase.PostCreateContacts, callback);
return(inputDeps);
}
public void Execute(int workItemIndex)
{
OutputStream.Begin(workItemIndex);
while (ManifoldIterator.HasItemsLeft())
{
ContactHeader contactHeader = ManifoldIterator.GetNextContactHeader();
for (int c = 0; c < contactHeader.NumContacts; c++)
{
Unity.Physics.ContactPoint contact = ManifoldIterator.GetNextContact();
float3 x0 = contact.Position;
float3 x1 = contactHeader.Normal * contact.Distance;
Color color = Color.green;
OutputStream.Arrow(x0, x1, color);
}
}
OutputStream.End();
}
public ContactPoint this[int contactIndex]
{
get
{
Assert.IsTrue(contactIndex >= 0 && contactIndex < k_MaxNumContacts);
int offset = contactIndex * 3;
var contact = new ContactPoint();
fixed(float *positions = m_ContactPositions)
{
contact.Position = *(float3 *)(positions + offset);
}
fixed(float *distances = m_Distances)
{
contact.Distance = distances[contactIndex];
}
return(contact);
}
set
{
Assert.IsTrue(contactIndex >= 0 && contactIndex < k_MaxNumContacts);
int offset = contactIndex * 3;
fixed(float *positions = m_ContactPositions)
{
*(float3 *)(positions + offset) = value.Position;
}
fixed(float *distances = m_Distances)
{
distances[contactIndex] = value.Distance;
}
}
}
public void UpdatePreviousContact(ContactPoint newData)
{
//<todo.eoin.hpmod Check last read was a contact
m_ContactReader.Write(newData);
}
