private void near(IntPtr space, IntPtr g1, IntPtr g2)
{
// no lock here! It's invoked from within Simulate(), which is thread-locked
if (m_global_contactcount >= maxContactsbeforedeath)
return;
// Test if we're colliding a geom with a space.
// If so we have to drill down into the space recursively
if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
return;
if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2))
{
// We'll be calling near recursivly if one
// of them is a space to find all of the
// contact points in the space
try
{
d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
}
catch (AccessViolationException)
{
m_log.Warn("[PHYSICS]: Unable to collide test a space");
return;
}
//here one should check collisions of geoms inside a space
// but on each space we only should have geoms that not colide amoung each other
// so we don't dig inside spaces
return;
}
// get geom bodies to check if we already a joint contact
// guess this shouldn't happen now
IntPtr b1 = d.GeomGetBody(g1);
IntPtr b2 = d.GeomGetBody(g2);
// d.GeomClassID id = d.GeomGetClass(g1);
// Figure out how many contact points we have
int count = 0;
try
{
// Colliding Geom To Geom
// This portion of the function 'was' blatantly ripped off from BoxStack.cs
if (g1 == g2)
return; // Can't collide with yourself
if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact))
return;
count = d.CollidePtr(g1, g2, (contactsPerCollision & 0xffff), ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
}
catch (SEHException)
{
m_log.Error("[PHYSICS]: The Operating system shut down ODE because of corrupt memory. This could be a result of really irregular terrain. If this repeats continuously, restart using Basic Physics and terrain fill your terrain. Restarting the sim.");
// ode.drelease(world);
base.TriggerPhysicsBasedRestart();
}
catch (Exception e)
{
m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message);
return;
}
// no contacts so done
if (count == 0)
return;
// now we have a contact describing colision of 2 things
// but with my changes now we don't know what they are
// so code gets more complex now
// try get physical actors
PhysicsActor p1;
PhysicsActor p2;
if (!actor_name_map.TryGetValue(g1, out p1))
{
p1 = PANull;
}
if (!actor_name_map.TryGetValue(g2, out p2))
{
p2 = PANull;
}
// update actors collision score
if (p1.CollisionScore >= float.MaxValue - count)
p1.CollisionScore = 0;
p1.CollisionScore += count;
if (p2.CollisionScore >= float.MaxValue - count)
p2.CollisionScore = 0;
p2.CollisionScore += count;
// get geoms names
String name1 = null;
String name2 = null;
if (!geom_name_map.TryGetValue(g1, out name1))
{
name1 = "null";
}
if (!geom_name_map.TryGetValue(g2, out name2))
{
name2 = "null";
}
ContactPoint maxDepthContact = new ContactPoint();
d.ContactGeom curContact = new d.ContactGeom();
float mu;
float bounce;
ContactData contactdata1;
ContactData contactdata2;
for (int i = 0; i < count; i++)
{
if (!GetCurContactGeom(i, ref curContact))
break;
if (curContact.depth <= 0)
continue;
if(curContact.g1 == IntPtr.Zero)
curContact.g1 = g1;
if(curContact.g2 == IntPtr.Zero)
curContact.g2 = g2;
//for debug d.Quaternion qtmp = d.BodyGetQuaternion(b1);
if (curContact.depth > maxDepthContact.PenetrationDepth)
{
maxDepthContact = new ContactPoint(
new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z),
new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z),
curContact.depth
);
}
IntPtr Joint;
// inform actors about colision
if (p1 is OdeCharacter && p2.PhysicsActorType == (int)ActorTypes.Prim)
{
// Testing if the collision is at the feet of the avatar
if ((p1.Position.Z - curContact.pos.Z) > (p1.Size.Z - avCapRadius) * 0.5f)
p1.IsColliding = true;
}
else
{
p1.IsColliding = true;
}
switch (p2.PhysicsActorType)
{
case (int)ActorTypes.Agent:
p1.CollidingObj = true;
break;
case (int)ActorTypes.Prim:
if (p1.Velocity.LengthSquared() > 0.0f)
p1.CollidingObj = true;
break;
case (int)ActorTypes.Unknown:
p1.CollidingGround = true;
break;
default:
p1.CollidingGround = true;
break;
}
if (p2 is OdeCharacter && p1.PhysicsActorType == (int)ActorTypes.Prim)
{
// Testing if the collision is at the feet of the avatar
if ((p2.Position.Z - curContact.pos.Z) > (p2.Size.Z - avCapRadius) * 0.5f)
p2.IsColliding = true;
}
else
{
p2.IsColliding = true;
}
switch (p1.PhysicsActorType)
{
case (int)ActorTypes.Agent:
p2.CollidingObj = true;
break;
case (int)ActorTypes.Prim:
if (p2.Velocity.LengthSquared() > 0.0f)
p2.CollidingObj = true;
break;
case (int)ActorTypes.Unknown:
p2.CollidingGround = true;
break;
default:
p2.CollidingGround = true;
break;
}
if (m_global_contactcount >= maxContactsbeforedeath)
break;
// we don't want prim or avatar to explode
// not in use section, so removed see older commits if needed
// skip actors with volumeDetect
Boolean skipThisContact = false;
if ((p1 is OdePrim) && (((OdePrim)p1).m_isVolumeDetect))
skipThisContact = true; // No collision on volume detect prims
if (!skipThisContact && (p2 is OdePrim) && (((OdePrim)p2).m_isVolumeDetect))
skipThisContact = true; // No collision on volume detect prims
// if (!skipThisContact && curContact.depth < 0f)
// skipThisContact = true;
// if (!skipThisContact && checkDupe(curContact, p2.PhysicsActorType))
// skipThisContact = true;
Joint = IntPtr.Zero;
if (!skipThisContact)
{
// If we're colliding against terrain
if (name1 == "Terrain")
{
// avatar to ground
/* not done by ode
if (p2.PhysicsActorType == (int)ActorTypes.Agent)
{
// If we're moving
if (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f)
{
float mu = AvatarMovementTerrainContactSurf.mu;
float bounce = AvatarMovementTerrainContactSurf.bounce;
float soft_cfm = AvatarMovementTerrainContactSurf.soft_cfm;
float soft_erp = AvatarMovementTerrainContactSurf.soft_erp;
doJoint = SetGlobalContact(ref curContact, mu, bounce, soft_cfm, soft_erp);
}
else
{
// Use the non moving terrain contact
float mu = TerrainContactSurf.mu;
float bounce = TerrainContactSurf.bounce;
float soft_cfm = TerrainContactSurf.soft_cfm;
float soft_erp = TerrainContactSurf.soft_erp;
doJoint = SetGlobalContact(ref curContact, mu, bounce, soft_cfm, soft_erp);
}
}
else
*/
if (p2.PhysicsActorType == (int)ActorTypes.Prim)
{
// prim terrain contact
contactdata2 = p2.ContactData;
bounce = contactdata2.bounce * TerrainBounce;
mu = (float)Math.Sqrt(contactdata2.mu * TerrainFriction);
if (Math.Abs(p2.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y) > 0.1f)
mu *= frictionMovementMult;
Joint = CreateContacJoint(ref curContact, mu, bounce, true);
}
}
else if (name2 == "Terrain")
{
// avatar to ground
/* not done by ode
if (p1.PhysicsActorType == (int)ActorTypes.Agent)
{
// If we're moving
if (Math.Abs(p1.Velocity.X) > 0.01f || Math.Abs(p1.Velocity.Y) > 0.01f)
{
// Use the movement terrain contact
float mu = AvatarMovementTerrainContactSurf.mu;
float bounce = AvatarMovementTerrainContactSurf.bounce;
float soft_cfm = AvatarMovementTerrainContactSurf.soft_cfm;
float soft_erp = AvatarMovementTerrainContactSurf.soft_erp;
doJoint = SetGlobalContact(ref curContact, mu, bounce, soft_cfm, soft_erp);
}
else
{
// Use the non moving terrain contact
float mu = TerrainContactSurf.mu;
float bounce = TerrainContactSurf.bounce;
float soft_cfm = TerrainContactSurf.soft_cfm;
float soft_erp = TerrainContactSurf.soft_erp;
doJoint = SetGlobalContact(ref curContact, mu, bounce, soft_cfm, soft_erp);
}
}
else
*/
if (p1.PhysicsActorType == (int)ActorTypes.Prim)
{
// prim terrain contact
contactdata1 = p1.ContactData;
bounce = contactdata1.bounce * TerrainBounce;
mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction);
if (Math.Abs(p1.Velocity.X) > 0.1f || Math.Abs(p1.Velocity.Y) > 0.1f)
mu *= frictionMovementMult;
Joint = CreateContacJoint(ref curContact, mu, bounce, true);
}
}
// collisions with water
else if (name1 == "Water" || name2 == "Water")
{
if (curContact.depth > 0.1f)
{
curContact.depth *= 52;
//contact.normal = new d.Vector3(0, 0, 1);
//contact.pos = new d.Vector3(0, 0, contact.pos.Z - 5f);
}
mu = 0;
bounce = 0;
Joint = CreateContacJoint(ref curContact, mu, bounce,true);
}
else
{
// we're colliding with prim or avatar
if (p1 != null && p2 != null)
{
contactdata1 = p1.ContactData;
contactdata2 = p2.ContactData;
bool erpsoft;
// avatars fail colisions if 2 soft
if (p1.PhysicsActorType == (int)ActorTypes.Agent || p2.PhysicsActorType == (int)ActorTypes.Agent)
erpsoft = false;
else
erpsoft = true;
bounce = contactdata1.bounce * contactdata2.bounce;
mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
mu *= frictionMovementMult;
Joint = CreateContacJoint(ref curContact, mu, bounce, erpsoft);
}
}
if (Joint != IntPtr.Zero)
{
m_global_contactcount++;
d.JointAttach(Joint, b1, b2);
}
}
}
// this was inside above loop ?
collision_accounting_events(p1, p2, maxDepthContact);
/*
if (notskipedcount > geomContactPointsStartthrottle)
{
// If there are more then 3 contact points, it's likely
// that we've got a pile of objects, so ...
// We don't want to send out hundreds of terse updates over and over again
// so lets throttle them and send them again after it's somewhat sorted out.
this needs checking so out for now
if (b1 != IntPtr.Zero)
p1.ThrottleUpdates = true;
if (b2 != IntPtr.Zero)
p2.ThrottleUpdates = true;
}
*/
}
// This is the standard Near. g2 is the ray
private void near(IntPtr space, IntPtr g1, IntPtr g2)
{
//Don't test against heightfield Geom, or you'll be sorry!
// Exclude heightfield geom
if (g1 == IntPtr.Zero || g1 == g2)
return;
if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass)
return;
// Raytest against AABBs of spaces first, then dig into the spaces it hits for actual geoms.
if (d.GeomIsSpace(g1))
{
try
{
d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
}
catch (Exception e)
{
m_log.WarnFormat("[PHYSICS Ray]: Unable to Space collide test an object: {0}", e.Message);
}
return;
}
int count = 0;
try
{
count = d.CollidePtr(g1, g2, ColisionContactGeomsPerTest, m_scene.ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
}
catch (SEHException)
{
m_log.Error("[PHYSICS Ray]: The Operating system shut down ODE because of corrupt memory. This could be a result of really irregular terrain. If this repeats continuously, restart using Basic Physics and terrain fill your terrain. Restarting the sim.");
}
catch (Exception e)
{
m_log.WarnFormat("[PHYSICS Ray]: Unable to collide test an object: {0}", e.Message);
return;
}
if (count == 0)
return;
PhysicsActor p1 = null;
if (g1 != IntPtr.Zero)
m_scene.actor_name_map.TryGetValue(g1, out p1);
d.ContactGeom curcontact = new d.ContactGeom();
// Loop over contacts, build results.
for (int i = 0; i < count; i++)
{
if (!GetCurContactGeom(i, ref curcontact))
break;
if (p1 != null) {
if (p1 is OdePrim)
{
ContactResult collisionresult = new ContactResult();
collisionresult.ConsumerID = ((OdePrim)p1).m_localID;
collisionresult.Pos = new Vector3(curcontact.pos.X, curcontact.pos.Y, curcontact.pos.Z);
collisionresult.Depth = curcontact.depth;
collisionresult.Normal = new Vector3(curcontact.normal.X, curcontact.normal.Y,
curcontact.normal.Z);
lock (m_contactResults)
m_contactResults.Add(collisionresult);
}
}
}
}
/// <summary>
/// This is our near callback. A geometry is near a body
/// </summary>
/// <param name="space">The space that contains the geoms. Remember, spaces are also geoms</param>
/// <param name="g1">a geometry or space</param>
/// <param name="g2">another geometry or space</param>
///
private void near(IntPtr space, IntPtr g1, IntPtr g2)
{
// no lock here! It's invoked from within Simulate(), which is thread-locked
if (m_global_contactcount >= maxContactsbeforedeath)
return;
// Test if we're colliding a geom with a space.
// If so we have to drill down into the space recursively
if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
return;
if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2))
{
// We'll be calling near recursivly if one
// of them is a space to find all of the
// contact points in the space
try
{
d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
}
catch (AccessViolationException)
{
m_log.Warn("[PHYSICS]: Unable to collide test a space");
return;
}
//here one should check collisions of geoms inside a space
// but on each space we only should have geoms that not colide amoung each other
// so we don't dig inside spaces
return;
}
// get geom bodies to check if we already a joint contact
// guess this shouldn't happen now
IntPtr b1 = d.GeomGetBody(g1);
IntPtr b2 = d.GeomGetBody(g2);
// d.GeomClassID id = d.GeomGetClass(g1);
// Figure out how many contact points we have
int count = 0;
try
{
// Colliding Geom To Geom
// This portion of the function 'was' blatantly ripped off from BoxStack.cs
if (g1 == g2)
return; // Can't collide with yourself
if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact))
return;
/*
// debug
PhysicsActor dp2;
if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass)
{
d.AABB aabb;
d.GeomGetAABB(g2, out aabb);
float x = aabb.MaxX - aabb.MinX;
float y = aabb.MaxY - aabb.MinY;
float z = aabb.MaxZ - aabb.MinZ;
if (x > 60.0f || y > 60.0f || z > 60.0f)
{
if (!actor_name_map.TryGetValue(g2, out dp2))
m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2");
else
m_log.WarnFormat("[PHYSICS]: land versus large prim geo {0},size {1}, AABBsize <{2},{3},{4}>, at {5} ori {6},({7})",
dp2.Name, dp2.Size, x, y, z,
dp2.Position.ToString(),
dp2.Orientation.ToString(),
dp2.Orientation.Length());
return;
}
}
//
*/
if (d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc ||
d.GeomGetCategoryBits(g2) == (uint)CollisionCategories.VolumeDtc)
{
int cflags;
unchecked
{
cflags = (int)(1 | d.CONTACTS_UNIMPORTANT);
}
count = d.CollidePtr(g1, g2, cflags, ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
}
else
count = d.CollidePtr(g1, g2, (contactsPerCollision & 0xffff), ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
}
catch (SEHException)
{
m_log.Error("[PHYSICS]: The Operating system shut down ODE because of corrupt memory. This could be a result of really irregular terrain. If this repeats continuously, restart using Basic Physics and terrain fill your terrain. Restarting the sim.");
// ode.drelease(world);
base.TriggerPhysicsBasedRestart();
}
catch (Exception e)
{
m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message);
return;
}
// contacts done
if (count == 0)
return;
// try get physical actors
PhysicsActor p1;
PhysicsActor p2;
if (!actor_name_map.TryGetValue(g1, out p1))
{
m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 1");
return;
}
if (!actor_name_map.TryGetValue(g2, out p2))
{
m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2");
return;
}
// update actors collision score
if (p1.CollisionScore >= float.MaxValue - count)
p1.CollisionScore = 0;
p1.CollisionScore += count;
if (p2.CollisionScore >= float.MaxValue - count)
p2.CollisionScore = 0;
p2.CollisionScore += count;
// get first contact
d.ContactGeom curContact = new d.ContactGeom();
if (!GetCurContactGeom(0, ref curContact))
return;
ContactPoint maxDepthContact = new ContactPoint();
// do volume detection case
if ((p1.IsVolumeDtc || p2.IsVolumeDtc))
{
maxDepthContact = new ContactPoint(
new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z),
new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z),
curContact.depth, false
);
collision_accounting_events(p1, p2, maxDepthContact);
return;
}
// big messy collision analises
float mu = 0;
float bounce = 0;
// bool IgnoreNegSides = false;
ContactData contactdata1 = new ContactData(0, 0, false);
ContactData contactdata2 = new ContactData(0, 0, false);
bool dop1ava = false;
bool dop2ava = false;
bool ignore = false;
bool smoothMesh = false;
switch (p1.PhysicsActorType)
{
case (int)ActorTypes.Agent:
{
dop1ava = true;
switch (p2.PhysicsActorType)
{
case (int)ActorTypes.Agent:
case (int)ActorTypes.Prim:
break;
default:
ignore = true; // avatar to terrain and water ignored
break;
}
break;
}
case (int)ActorTypes.Prim:
{
switch (p2.PhysicsActorType)
{
case (int)ActorTypes.Agent:
dop2ava = true;
break;
case (int)ActorTypes.Prim:
Vector3 relV = p1.rootVelocity - p2.rootVelocity;
float relVlenSQ = relV.LengthSquared();
if (relVlenSQ > 0.0001f)
{
p1.CollidingObj = true;
p2.CollidingObj = true;
}
p1.getContactData(ref contactdata1);
p2.getContactData(ref contactdata2);
bounce = contactdata1.bounce * contactdata2.bounce;
mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
if (relVlenSQ > 0.01f)
mu *= frictionMovementMult;
if(d.GeomGetClass(g2) == d.GeomClassID.TriMeshClass &&
d.GeomGetClass(g1) == d.GeomClassID.TriMeshClass)
smoothMesh = true;
break;
case (int)ActorTypes.Ground:
p1.getContactData(ref contactdata1);
bounce = contactdata1.bounce * TerrainBounce;
mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction);
Vector3 v1 = p1.rootVelocity;
if (Math.Abs(v1.X) > 0.1f || Math.Abs(v1.Y) > 0.1f)
mu *= frictionMovementMult;
p1.CollidingGround = true;
if(d.GeomGetClass(g1) == d.GeomClassID.TriMeshClass)
smoothMesh = true;
break;
case (int)ActorTypes.Water:
default:
ignore = true;
break;
}
}
break;
case (int)ActorTypes.Ground:
if (p2.PhysicsActorType == (int)ActorTypes.Prim)
{
p2.CollidingGround = true;
p2.getContactData(ref contactdata2);
bounce = contactdata2.bounce * TerrainBounce;
mu = (float)Math.Sqrt(contactdata2.mu * TerrainFriction);
// if (curContact.side1 > 0) // should be 2 ?
// IgnoreNegSides = true;
Vector3 v2 = p2.rootVelocity;
if (Math.Abs(v2.X) > 0.1f || Math.Abs(v2.Y) > 0.1f)
mu *= frictionMovementMult;
if(d.GeomGetClass(g2) == d.GeomClassID.TriMeshClass)
smoothMesh = true;
}
else
ignore = true;
break;
case (int)ActorTypes.Water:
default:
break;
}
if (ignore)
return;
IntPtr Joint;
bool FeetCollision = false;
int ncontacts = 0;
int i = 0;
maxDepthContact = new ContactPoint();
maxDepthContact.PenetrationDepth = float.MinValue;
ContactPoint minDepthContact = new ContactPoint();
minDepthContact.PenetrationDepth = float.MaxValue;
SharedTmpcontact.geom.depth = 0;
SharedTmpcontact.surface.mu = mu;
SharedTmpcontact.surface.bounce = bounce;
d.ContactGeom altContact = new d.ContactGeom();
bool useAltcontact = false;
bool noskip = true;
if(dop1ava || dop2ava)
smoothMesh = false;
while (true)
{
noskip = true;
useAltcontact = false;
if (dop1ava)
{
if ((((OdeCharacter)p1).Collide(g1, g2, false, ref curContact, ref altContact , ref useAltcontact, ref FeetCollision)))
{
if (p2.PhysicsActorType == (int)ActorTypes.Agent)
{
p1.CollidingObj = true;
p2.CollidingObj = true;
}
else if (p2.rootVelocity.LengthSquared() > 0.0f)
p2.CollidingObj = true;
}
else
noskip = false;
}
else if (dop2ava)
{
if ((((OdeCharacter)p2).Collide(g2, g1, true, ref curContact, ref altContact , ref useAltcontact, ref FeetCollision)))
{
if (p1.PhysicsActorType == (int)ActorTypes.Agent)
{
p1.CollidingObj = true;
p2.CollidingObj = true;
}
else if (p1.rootVelocity.LengthSquared() > 0.0f)
p1.CollidingObj = true;
}
else
noskip = false;
}
if (noskip)
{
if(useAltcontact)
Joint = CreateContacJoint(ref altContact,smoothMesh);
else
Joint = CreateContacJoint(ref curContact,smoothMesh);
if (Joint == IntPtr.Zero)
break;
d.JointAttach(Joint, b1, b2);
ncontacts++;
if (curContact.depth > maxDepthContact.PenetrationDepth)
{
maxDepthContact.Position.X = curContact.pos.X;
maxDepthContact.Position.Y = curContact.pos.Y;
maxDepthContact.Position.Z = curContact.pos.Z;
maxDepthContact.PenetrationDepth = curContact.depth;
maxDepthContact.CharacterFeet = FeetCollision;
}
if (curContact.depth < minDepthContact.PenetrationDepth)
{
minDepthContact.PenetrationDepth = curContact.depth;
minDepthContact.SurfaceNormal.X = curContact.normal.X;
minDepthContact.SurfaceNormal.Y = curContact.normal.Y;
minDepthContact.SurfaceNormal.Z = curContact.normal.Z;
}
}
if (++i >= count)
break;
if (!GetCurContactGeom(i, ref curContact))
break;
}
if (ncontacts > 0)
{
maxDepthContact.SurfaceNormal.X = minDepthContact.SurfaceNormal.X;
maxDepthContact.SurfaceNormal.Y = minDepthContact.SurfaceNormal.Y;
maxDepthContact.SurfaceNormal.Z = minDepthContact.SurfaceNormal.Z;
collision_accounting_events(p1, p2, maxDepthContact);
}
}
