public override float AddSingleResult(ManifoldPoint cp, CollisionObjectWrapper colObj0Wrap, int partId0, int index0, CollisionObjectWrapper colObj1Wrap, int partId1, int index1)
{
return(1.0f);
/*
* CollisionObject col = null;
* if (colObj0Wrap.CollisionObject == colObj.BulletCollisionObject) col = (CollisionObject)colObj1Wrap.CollisionObject.UserObject;
* else if (colObj1Wrap.CollisionObject == colObj.BulletCollisionObject) col = (CollisionObject)colObj0Wrap.CollisionObject.UserObject;
*
* var com = (GameComponent)col.tag;
* if (com.GameObject == gameObject) return 0.0f;
*
* var collision = new Collision();
* collision.GameObject = com.GameObject;
* collision.Collider = com as Collider;
* collision.RigidBody = com as MikuMikuWorld.GameComponents.RigidBody;
* collision.Impulse = cp.AppliedImpulse;
*
* if (Collides.Exists((c) =>
* {
* if (collision.Collider != null && c.Collider == collision.Collider) return true;
* else if (collision.RigidBody != null && c.RigidBody == collision.RigidBody) return true;
* return false;
* }))
* return 0.0f;
*
* Collides.Add(collision);
*
* return cp.CombinedFriction;*/
}
//Collision Added Callback
private void OnContactProcessed(ManifoldPoint cp, CollisionObject body0, CollisionObject body1)
{
if (Equals(body0, BRigidBody.BodyInstance) || Equals(body1, BRigidBody.BodyInstance))
{
OnContactAdded(body0);
}
}
private void ContactAdded(ManifoldPoint cp, CollisionObjectWrapper colObj0Wrap, int partId0, int index0, CollisionObjectWrapper colObj1Wrap, int partId1, int index1)
{
float impulse = cp.AppliedImpulse;
if (impulse < 0.4f)
{
return;
}
impulse -= 0.4f;
impulse = Math.Min(impulse, 4.0f) * 0.25f;
string sound0 = colObj0Wrap.CollisionObject.UserObject as string;
if (sound0 != null)
{
_audioEngine.SetVolume(sound0, impulse);
_audioEngine.Play(sound0);
}
string sound1 = colObj1Wrap.CollisionObject.UserObject as string;
if (sound1 != null)
{
_audioEngine.SetVolume(sound1, impulse);
_audioEngine.Play(sound1);
}
}
public static void TickCallback(DynamicsWorld world, float timeStep)
{
int numManifolds = colWorld.Dispatcher.NumManifolds;
for (int i = 0; i < numManifolds; i++)
{
PersistentManifold contactManifold = colWorld.Dispatcher.GetManifoldByIndexInternal(i);
// CollisionObject obA = contactManifold.Body0 as CollisionObject;
// CollisionObject obB = contactManifold.Body1 as CollisionObject;
var entity1 = (contactManifold.Body0 as RigidBody).UserObject as Entity;
var entity2 = (contactManifold.Body1 as RigidBody).UserObject as Entity;
int numContacts = contactManifold.NumContacts;
for (int j = 0; j < numContacts; j++)
{
ManifoldPoint pt = contactManifold.GetContactPoint(j);
if (pt.Distance < 1.0f)
{
if (HardCollisionAction != null)
{
if (entity1.CollisionType == CollisionTypeEnum.Soft &&
entity2.CollisionType == CollisionTypeEnum.Soft)
{
SoftCollisionAction();
}
else
{
HardCollisionAction();
}
}
return;
}
}
}
}
private void OnContactAdded(ManifoldPoint cp, CollisionObjectWrapper colObj0Wrap, int partId0, int index0,
CollisionObjectWrapper colObj1Wrap, int partId1, int index1)
{
//Debug.WriteLine("OnContactAdded");
int numManifolds = BtWorld.Dispatcher.NumManifolds;
for (int i = 0; i < numManifolds; i++)
{
PersistentManifold contactManifold = BtWorld.Dispatcher.GetManifoldByIndexInternal(i);
int numContacts = contactManifold.NumContacts;
if (numContacts > 0)
{
cp.UserPersistentData = 1;
CollisionObject obA = (CollisionObject)contactManifold.Body0;
CollisionObject obB = (CollisionObject)contactManifold.Body1;
RigidBody btRigidBodyA = (RigidBody)obA;
RigidBody btRigidBodyB = (RigidBody)obB;
RigidBodyImp rigidBodyA = new RigidBodyImp();
RigidBodyImp rigidBodyB = new RigidBodyImp();
rigidBodyA._rbi = btRigidBodyA;
rigidBodyB._rbi = btRigidBodyB;
rigidBodyA.OnCollision(rigidBodyB);
}
}
}
public void StoreImpulses()
{
for (int i = 0; i < _constraintCount; ++i)
{
ContactConstraint c = Constraints[i];
if (c.BodyA.Penetrable || c.BodyB.Penetrable)
{
continue;
}
Manifold m = c.Manifold;
for (int j = 0; j < c.PointCount; ++j)
{
ManifoldPoint pj = m.Points[j];
ContactConstraintPoint cp = c.Points[j];
pj.NormalImpulse = cp.NormalImpulse;
pj.TangentImpulse = cp.TangentImpulse;
m.Points[j] = pj;
}
c.Manifold = m;
_contacts[i].Manifold = m;
}
}
public void Report(ContactConstraint[] constraints)
{
if (this._listener != null)
{
for (int i = 0; i < this._contactCount; i++)
{
Contact contact = this._contacts[i];
ContactConstraint contactConstraint = constraints[i];
ContactResult contactResult = new ContactResult();
contactResult.Shape1 = contact.GetShape1();
contactResult.Shape2 = contact.GetShape2();
Body body = contactResult.Shape1.GetBody();
int manifoldCount = contact.GetManifoldCount();
Manifold[] manifolds = contact.GetManifolds();
for (int j = 0; j < manifoldCount; j++)
{
Manifold manifold = manifolds[j];
contactResult.Normal = manifold.Normal;
for (int k = 0; k < manifold.PointCount; k++)
{
ManifoldPoint manifoldPoint = manifold.Points[k];
ContactConstraintPoint contactConstraintPoint = contactConstraint.Points[k];
contactResult.Position = body.GetWorldPoint(manifoldPoint.LocalPoint1);
contactResult.NormalImpulse = contactConstraintPoint.NormalImpulse;
contactResult.TangentImpulse = contactConstraintPoint.TangentImpulse;
contactResult.ID = manifoldPoint.ID;
this._listener.Result(contactResult);
}
}
}
}
}
public void Evaluate(int SpreadMax)
{
if (this.FContactPoints.PluginIO.IsConnected)
{
this.FPointWorld1.SliceCount = SpreadMax;
this.FPointWorld2.SliceCount = SpreadMax;
this.FLifeTime.SliceCount = SpreadMax;
this.FImpulse.SliceCount = SpreadMax;
for (int i = 0; i < SpreadMax; i++)
{
ManifoldPoint pt = this.FContactPoints[i];
this.FPointWorld1[i] = pt.PositionWorldOnA.ToVVVVector();
this.FPointWorld2[i] = pt.PositionWorldOnB.ToVVVVector();
this.FLifeTime[i] = pt.LifeTime;
this.FImpulse[i] = pt.AppliedImpulse;
}
}
else
{
this.FPointWorld1.SliceCount = 0;
this.FPointWorld2.SliceCount = 0;
this.FLifeTime.SliceCount = 0;
this.FImpulse.SliceCount = 0;
}
}
static void CustomMaterialCombinerCallback(ManifoldPoint cp,
CollisionObjectWrapper colObj0Wrap, int partId0, int index0,
CollisionObjectWrapper colObj1Wrap, int partId1, int index1)
{
// Apply material properties
if (colObj0Wrap.CollisionShape.ShapeType == BroadphaseNativeType.TriangleShape)
{
CollisionShape parent0 = colObj0Wrap.CollisionObject.CollisionShape;
if (parent0 != null && parent0.ShapeType == BroadphaseNativeType.MultiMaterialTriangleMesh)
{
MultimaterialTriangleMeshShape shape = parent0 as MultimaterialTriangleMeshShape;
BulletMaterial props = shape.GetMaterialProperties(partId0, index0);
cp.CombinedFriction = CalculateCombinedFriction(props.Friction, colObj1Wrap.CollisionObject.Friction);
cp.CombinedRestitution = props.Restitution * colObj1Wrap.CollisionObject.Restitution;
}
}
else if (colObj1Wrap.CollisionShape.ShapeType == BroadphaseNativeType.TriangleShape)
{
CollisionShape parent1 = colObj1Wrap.CollisionObject.CollisionShape;
if (parent1 != null && parent1.ShapeType == BroadphaseNativeType.MultiMaterialTriangleMesh)
{
MultimaterialTriangleMeshShape shape = parent1 as MultimaterialTriangleMeshShape;
BulletMaterial props = shape.GetMaterialProperties(partId1, index1);
cp.CombinedFriction = CalculateCombinedFriction(props.Friction, colObj0Wrap.CollisionObject.Friction);
cp.CombinedRestitution = props.Restitution * colObj0Wrap.CollisionObject.Restitution;
}
}
}
public BulletUnityContactPoint(ManifoldPoint mp, CollisionObjectWrapper a, CollisionObjectWrapper b)
{
objA = a.CollisionObject;
objB = b.CollisionObject;
shapeA = a.CollisionShape;
shapeB = b.CollisionShape;
AppliedImpulse = mp.AppliedImpulse;
AppliedImpulseLateral1 = mp.AppliedImpulseLateral1;
AppliedImpulseLateral2 = mp.AppliedImpulseLateral2;
CombinedFriction = mp.CombinedFriction;
CombinedRestitution = mp.CombinedRestitution;
CombinedRollingFriction = mp.CombinedRollingFriction;
ContactCfm1 = mp.ContactCfm1;
ContactCfm2 = mp.ContactCfm2;
ContactMotion1 = mp.ContactMotion1;
ContactMotion2 = mp.ContactMotion2;
Distance = mp.Distance;
Distance1 = mp.Distance1;
Index0 = mp.Index0;
Index1 = mp.Index1;
PartID0 = mp.PartId0;
PartID1 = mp.PartId1;
LateralFrictionDir1 = mp.LateralFrictionDir1.ToUnity();
LateralFrictionDir2 = mp.LateralFrictionDir2.ToUnity();
Lifetime = mp.LifeTime;
NormalWorldOnB = mp.NormalWorldOnB.ToUnity();
PositionWorldOnA = mp.PositionWorldOnA.ToUnity();
PositionWorldOnB = mp.PositionWorldOnB.ToUnity();
}
protected float Solve(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw)
{
float maxImpulse = 0;
Vector3 color = new Vector3(0, 1, 0);
if (cp.Distance <= 0)
{
if (iter == 0)
{
if (debugDraw != null)
{
debugDraw.DrawContactPoint(cp.PositionWorldOnB, cp.NormalWorldOnB, cp.Distance, cp.LifeTime, color);
}
}
ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData;
float impulse = cpd.ContactSolverFunc(
bodyA, bodyB,
cp,
info);
if (maxImpulse < impulse)
{
maxImpulse = impulse;
}
}
return(maxImpulse);
}
internal static void Update(float deltaTime)
{
world.StepSimulation(deltaTime, 100);
world.PerformDiscreteCollisionDetection();
for (var i = 0; i < world.Dispatcher.NumManifolds; i++)
{
var mani = world.Dispatcher.GetManifoldByIndexInternal(i);
mani.RefreshContactPoints(mani.Body0.WorldTransform, mani.Body1.WorldTransform);
var colA = (mani.Body0.UserObject as CollisionObject).tag as PhysicalGameComponent;
var colB = (mani.Body1.UserObject as CollisionObject).tag as PhysicalGameComponent;
if (colA == colB)
{
continue;
}
var mps = new ManifoldPoint[mani.NumContacts];
for (int j = 0; j < mani.NumContacts; j++)
{
mps[j] = mani.GetContactPoint(j);
}
colA.OnCollision(colB.GameObject, mps);
colB.OnCollision(colA.GameObject, mps);
}
}
public bool Callback(ref ManifoldPoint cp, CollisionObject colObj0, int partId0, int index0, CollisionObject colObj1, int partId1, int index1)
{
float friction0 = colObj0.GetFriction();
float friction1 = colObj1.GetFriction();
float restitution0 = colObj0.GetRestitution();
float restitution1 = colObj1.GetRestitution();
if ((colObj0.GetCollisionFlags() & CollisionFlags.CF_CUSTOM_MATERIAL_CALLBACK) != 0)
{
friction0 = 1.0f; //partId0,index0
restitution0 = 0.0f;
}
if ((colObj1.GetCollisionFlags() & CollisionFlags.CF_CUSTOM_MATERIAL_CALLBACK) != 0)
{
if ((index1 & 1) != 0)
{
friction1 = 1.0f; //partId1,index1
}
else
{
friction1 = 0f;
}
restitution1 = 0f;
}
cp.m_combinedFriction = CalculateCombinedFriction(friction0, friction1);
cp.m_combinedRestitution = calculateCombinedRestitution(restitution0, restitution1);
//this return value is currently ignored, but to be on the safe side: return false if you don't calculate friction
return(true);
}
public override float AddSingleResult(ManifoldPoint cp, CollisionObjectWrapper colObj0Wrap, int partId0, int index0,
CollisionObjectWrapper colObj1Wrap, int partId1, int index1)
{
var part = (Part)colObj1Wrap.CollisionObject.UserObject;
var didHitCharacter = part is Character;
var partPosition = part.CFrame.p;
var delta = partPosition - _parentPosition;
var dist = delta.magnitude;
var normal = delta == Vector3.Zero ? Vector3.Up : delta.unit;
normal = Vector3.Up;
var radius = part.Size.magnitude / 2;
var surfaceArea = radius * radius;
var impulse = normal * _pressure * surfaceArea * (1.0f / 4560.0f);
var frac = didHitCharacter ? 1 - Math.Max(0, Math.Min(1, (dist - 2) / _radius)) : 1;
impulse *= frac;
var torque = impulse * 0.5f * radius;
var bulletImpulse = new BulletSharp.Math.Vector3(impulse.x, impulse.y, impulse.z);
var bulletPosition = new BulletSharp.Math.Vector3(partPosition.X, partPosition.Y, partPosition.z);
var bulletTorque = new BulletSharp.Math.Vector3(torque.x, torque.y, torque.z);
part.RigidBody.ApplyImpulseRef(ref bulletImpulse, ref bulletPosition);
part.RigidBody.ApplyTorqueImpulseRef(ref bulletTorque);
_explosion.OnHit.Fire(part, dist);
return(0);
}
public override float AddSingleResult(ManifoldPoint cp,
CollisionObjectWrapper colObj0Wrap, int partId0, int index0,
CollisionObjectWrapper colObj1Wrap, int partId1, int index1)
{
//Vector3 ptA = cp.PositionWorldOnA;
//Vector3 ptB = cp.PositionWorldOnB;
//Vector3 color = new Vector3(255, 0, 255);
if (cp.Distance < 0.0f)
{
if (bodyCollision == colObj1Wrap.CollisionObject)
{
//Vector3 ptA = cp.PositionWorldOnA;
//Vector3 ptB = cp.PositionWorldOnB;
//Vector3 normalOnB = cp.NormalWorldOnB;
//bodyCollision.ApplyForce(new Vector3(255, 255, 255), new Vector3(0, 0, 0));
//bodyCollision.ApplyImpulse(new Vector3(100, 0, 100), new Vector3(0, 200, 0));
//_world.DebugDrawer.DrawLine(ref ptA, ref ptB, ref color);
//Console.WriteLine("normalOnB: (x " + normalOnB.X + ", y " + normalOnB.Y + ", z " + normalOnB.Z + ")");
Console.WriteLine("" + colObj1Wrap.CollisionObject.ToString());// "ptA.y: " + ptA.Y + "; ptB.y: " + ptB.Y);
}
}
return(0);
}
public override float AddSingleResult(ManifoldPoint cp, CollisionObject colObj0, int partId0, int index0, CollisionObject colObj1, int partId1, int index1)
{
if (colObj0.UserObject != null && colObj0.UserObject is IPhysicObject)
{
objs.Add(colObj0.UserObject as IPhysicObject);
}
return(0);
}
public override float AddSingleResult(ManifoldPoint cp, CollisionObjectWrapper colObj0Wrap, int partId0, int index0, CollisionObjectWrapper colObj1Wrap, int partId1, int index1)
{
if (cp.Distance <= margin)
{
m_connected = true;
}
return(1.0f);
}
public override float AddSingleResult(ManifoldPoint cp, CollisionObjectWrapper colObj0Wrap, int partId0, int index0, CollisionObjectWrapper colObj1Wrap, int partId1, int index1)
{
Vector3 ptA = cp.PositionWorldOnA;
Vector3 ptB = cp.PositionWorldOnB;
world.DebugDrawer.DrawLine(ref ptA, ref ptB, ref ptA);
return(0);
}
public static float ResolveSingleFrictionEmpty(
RigidBody bodyA,
RigidBody bodyB,
ManifoldPoint contactPoint,
ContactSolverInfo solverInfo)
{
return(0);
}
public bool RecoverFromPenetration(CollisionWorld collisionWorld)
{
bool penetration = false;
collisionWorld.GetDispatcher().DispatchAllCollisionPairs(m_ghostObject.GetOverlappingPairCache(), collisionWorld.GetDispatchInfo(), collisionWorld.GetDispatcher());
m_currentPosition = m_ghostObject.GetWorldTransform()._origin;
float maxPen = 0f;
for (int i = 0; i < m_ghostObject.GetOverlappingPairCache().GetNumOverlappingPairs(); i++)
{
m_manifoldArray.Clear();
BroadphasePair collisionPair = m_ghostObject.GetOverlappingPairCache().GetOverlappingPairArray()[i];
if (collisionPair.m_algorithm != null)
{
collisionPair.m_algorithm.GetAllContactManifolds(m_manifoldArray);
}
for (int j = 0; j < m_manifoldArray.Count; j++)
{
PersistentManifold manifold = m_manifoldArray[j];
float directionSign = manifold.GetBody0() == m_ghostObject ? -1f : 1f;
for (int p = 0; p < manifold.GetNumContacts(); p++)
{
ManifoldPoint pt = manifold.GetContactPoint(p);
float dist = pt.GetDistance();
if (dist < 0.0)
{
if (dist < maxPen)
{
maxPen = dist;
m_touchingNormal = pt.m_normalWorldOnB * directionSign; //??
}
m_currentPosition += pt.m_normalWorldOnB * directionSign * dist * 0.2f;
penetration = true;
}
else
{
//printf("touching %f\n", dist);
}
}
//manifold->clearManifold();
}
}
IndexedMatrix newTrans = m_ghostObject.GetWorldTransform();
newTrans._origin = m_currentPosition;
m_ghostObject.SetWorldTransform(ref newTrans);
// printf("m_touchingNormal = %f,%f,%f\n",m_touchingNormal[0],m_touchingNormal[1],m_touchingNormal[2]);
return(penetration);
}
private void ContactAdded(ManifoldPoint cp, CollisionObjectWrapper colObj0Wrap, int partId0, int index0, CollisionObjectWrapper colObj1Wrap, int partId1, int index1)
{
Assert.AreEqual(2, index0);
Assert.AreEqual(2, cp.Index0);
Assert.AreSame(compound, colObj0Wrap.CollisionObject);
Assert.AreSame(ground, colObj1Wrap.CollisionObject);
Assert.AreSame(boxShape3, colObj0Wrap.CollisionShape);
Assert.AreSame(groundShape, colObj1Wrap.CollisionShape);
}
public override float AddSingleResult(ManifoldPoint cp,
CollisionObjectWrapper colObj0Wrap, int partId0, int index0,
CollisionObjectWrapper colObj1Wrap, int partId1, int index1)
{
if (cp.Distance < 0.0f)
{
logica.esZombie((RigidBody)colObj1Wrap.CollisionObject, true);// esZombie() tiene efecto colateral con esta firma, simplemente muere
}
return(0);
}
public override float AddSingleResult(ManifoldPoint cp, CollisionObjectWrapper colObj0Wrap, int partId0, int index0, CollisionObjectWrapper colObj1Wrap, int partId1, int index1)
{
if (Shark.CharacterOnSight)
{
CharacterStatus.DamageReceived = Constants.DAMAGE_TO_CHARACTER;
Shark.ChangeSharkWay();
SoundManager.SharkAttack.play();
}
return(0);
}
public override float AddSingleResult(ManifoldPoint cp,
CollisionObjectWrapper colObj0Wrap, int partId0, int index0,
CollisionObjectWrapper colObj1Wrap, int partId1, int index1)
{
Vector3 ptA = cp.PositionWorldOnA;
Vector3 ptB = cp.PositionWorldOnB;
UnityEngine.Debug.Log("Contact!");
//world.DebugDrawer.DrawLine(ref ptA, ref ptB, ref ptA);
UnityEngine.Debug.LogFormat("{0}, {1} {2}", ptA, ptB, world);
return(0);
}
public override float AddSingleResult(ManifoldPoint cp,
CollisionObjectWrapper colObj0Wrap, int partId0, int index0,
CollisionObjectWrapper colObj1Wrap, int partId1, int index1)
{
if (cp.Distance < 0.0f)
{
if (logica.esZombieEnIsla((RigidBody)colObj1Wrap.CollisionObject)) // esZombieEnIsla() tiene efecto cuando es true
{
//Console.WriteLine("Colisione con la isla!!!, tengo que hacer algo aca");
}
}
return(0);
}
public override float AddSingleResult(ManifoldPoint cp,
CollisionObjectWrapper colObj0Wrap, int partId0, int index0,
CollisionObjectWrapper colObj1Wrap, int partId1, int index1)
{
if (cp.Distance < 0.0f)
{
if (logica.esApocalipsisZombie((RigidBody)colObj1Wrap.CollisionObject))// esApocalipsisZombie() tiene efecto cuando es true
{
play.gameOver();
}
}
return(0);
}
// MyContactCallback is just an example to show how to get access to the child shape that collided
void MyContactCallback(ManifoldPoint cp, CollisionObjectWrapper colObj0Wrap, int partId0, int index0, CollisionObjectWrapper colObj1Wrap, int partId1, int index1)
{
if (colObj0Wrap.CollisionObject.CollisionShape.ShapeType == BroadphaseNativeType.CompoundShape)
{
CompoundShape compound = colObj0Wrap.CollisionObject.CollisionShape as CompoundShape;
CollisionShape childShape = compound.GetChildShape(index0);
}
if (colObj1Wrap.CollisionObject.CollisionShape.ShapeType == BroadphaseNativeType.CompoundShape)
{
CompoundShape compound = colObj1Wrap.CollisionObject.CollisionShape as CompoundShape;
CollisionShape childShape = compound.GetChildShape(index1);
}
}
/// <summary>
/// Finds any robot collisions and adds them to the list of contact points.
/// </summary>
/// <param name="pm"></param>
public void OnVisitPersistentManifold(PersistentManifold pm)
{
if (!mainState.Tracking)
{
return;
}
if (framesPassed == -1)
{
framesPassed = physicsWorld.frameCount - lastFrameCount;
for (int i = 0; i < framesPassed; i++)
{
ContactPoints.Add(new List <ContactDescriptor>());
}
}
BRigidBody obA = pm.Body0.UserObject as BRigidBody;
BRigidBody obB = pm.Body1.UserObject as BRigidBody;
BRigidBody robotBody = obA != null && obA.gameObject.name.StartsWith("node") ? obA : obB != null && obB.gameObject.name.StartsWith("node") ? obB : null;
if (robotBody == null)
{
return;
}
if (pm.NumContacts < 1)
{
return;
}
int numContacts = pm.NumContacts;
for (int i = 0; i < Math.Min(framesPassed, numContacts); i++)
{
ManifoldPoint mp = pm.GetContactPoint(i);
ContactDescriptor cd = new ContactDescriptor
{
AppliedImpulse = mp.AppliedImpulse,
Position = (mp.PositionWorldOnA + mp.PositionWorldOnB) * 0.5f,
RobotBody = robotBody
};
if (ContactPoints[i] != null)
{
ContactPoints[i].Add(cd);
}
}
}
protected float SolveFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw)
{
Vector3 color = new Vector3(0, 1, 0);
if (cp.Distance <= 0)
{
ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData;
cpd.FrictionSolverFunc(
bodyA, bodyB,
cp,
info);
}
return(0);
}
private void ObstaculoCollision(PersistentManifold contactManifold, int misilId, int objetoId)
{
int numContacts = contactManifold.NumContacts;
for (int j = 0; j < numContacts; j++)
{
ManifoldPoint pt = contactManifold.GetContactPoint(j);
double ptdist = pt.Distance;
if (Math.Abs(ptdist) < epsilonContact)
{
listaColisionesObstaculoMisil.Add(new Colision(objetoId, misilId));
listaObstaculos[objetoId].Destruir();
}
}
}
public static float ResolveSingleFrictionOriginal(
RigidBody bodyA,
RigidBody bodyB,
ManifoldPoint contactPoint,
ContactSolverInfo solverInfo)
{
Vector3 posA = contactPoint.PositionWorldOnA;
Vector3 posB = contactPoint.PositionWorldOnB;
Vector3 relPosA = posA - bodyA.CenterOfMassPosition;
Vector3 relPosB = posB - bodyB.CenterOfMassPosition;
ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;
if (cpd == null)
throw new BulletException();
float combinedFriction = cpd.Friction;
float limit = cpd.AppliedImpulse * combinedFriction;
//if (contactPoint.m_appliedImpulse>0.f)
//friction
{
//apply friction in the 2 tangential directions
{
// 1st tangent
Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA);
Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB);
Vector3 vel = velA - velB;
float vrel = Vector3.Dot(cpd.FrictionWorldTangentialA, vel);
// calculate j that moves us to zero relative velocity
float j = -vrel * cpd.JacDiagABInvTangentA;
float total = cpd.AccumulatedTangentImpulseA + j;
if (limit < total)
total = limit;
if (total < -limit)
total = -limit;
j = total - cpd.AccumulatedTangentImpulseA;
cpd.AccumulatedTangentImpulseA = total;
bodyA.ApplyImpulse(j * cpd.FrictionWorldTangentialA, relPosA);
bodyB.ApplyImpulse(j * -cpd.FrictionWorldTangentialA, relPosB);
}
{
// 2nd tangent
Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA);
Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB);
Vector3 vel = velA - velB;
float vrel = Vector3.Dot(cpd.FrictionWorldTangentialB, vel);
// calculate j that moves us to zero relative velocity
float j = -vrel * cpd.JacDiagABInvTangentB;
float total = cpd.AccumulatedTangentImpulseB + j;
if (limit < total)
total = limit;
if (total < -limit)
total = -limit;
j = total - cpd.AccumulatedTangentImpulseB;
cpd.AccumulatedTangentImpulseB = total;
bodyA.ApplyImpulse(j * cpd.FrictionWorldTangentialB, relPosA);
bodyB.ApplyImpulse(j * -cpd.FrictionWorldTangentialB, relPosB);
}
}
return cpd.AppliedImpulse;
}
protected float SolveFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw)
{
Vector3 color = new Vector3(0, 1, 0);
if (cp.Distance <= 0)
{
ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData;
cpd.FrictionSolverFunc(
bodyA, bodyB,
cp,
info);
}
return 0;
}
public static float ResolveSingleFrictionEmpty(
RigidBody bodyA,
RigidBody bodyB,
ManifoldPoint contactPoint,
ContactSolverInfo solverInfo)
{
return 0;
}
//velocity + friction
//response between two dynamic objects with friction
public static float ResolveSingleCollisionCombined(
RigidBody bodyA,
RigidBody bodyB,
ManifoldPoint contactPoint,
ContactSolverInfo solverInfo)
{
Vector3 posA = contactPoint.PositionWorldOnA;
Vector3 posB = contactPoint.PositionWorldOnB;
Vector3 normal = contactPoint.NormalWorldOnB;
Vector3 relPosA = posA - bodyA.CenterOfMassPosition;
Vector3 relPosB = posB - bodyB.CenterOfMassPosition;
Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA);
Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB);
Vector3 vel = velA - velB;
float relVel;
relVel = Vector3.Dot(normal, vel);
float Kfps = 1f / solverInfo.TimeStep;
//float damping = solverInfo.m_damping;
float Kerp = solverInfo.Erp;
float Kcor = Kerp * Kfps;
ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;
if (cpd == null)
throw new BulletException();
float distance = cpd.Penetration;
float positionalError = Kcor * -distance;
float velocityError = cpd.Restitution - relVel;// * damping;
float penetrationImpulse = positionalError * cpd.JacDiagABInv;
float velocityImpulse = velocityError * cpd.JacDiagABInv;
float normalImpulse = penetrationImpulse + velocityImpulse;
// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
float oldNormalImpulse = cpd.AppliedImpulse;
float sum = oldNormalImpulse + normalImpulse;
cpd.AppliedImpulse = 0 > sum ? 0 : sum;
normalImpulse = cpd.AppliedImpulse - oldNormalImpulse;
if (bodyA.InverseMass != 0)
{
bodyA.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyA.InverseMass, cpd.AngularComponentA, normalImpulse);
}
if (bodyB.InverseMass != 0)
{
bodyB.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyB.InverseMass, cpd.AngularComponentB, -normalImpulse);
}
{
//friction
Vector3 vel12 = bodyA.GetVelocityInLocalPoint(relPosA);
Vector3 vel22 = bodyB.GetVelocityInLocalPoint(relPosB);
Vector3 vel3 = vel12 - vel22;
relVel = Vector3.Dot(normal, vel3);
Vector3 latVel = vel3 - normal * relVel;
float lat_rel_vel = latVel.Length();
float combinedFriction = cpd.Friction;
if (cpd.AppliedImpulse > 0)
if (lat_rel_vel > float.Epsilon)
{
latVel /= lat_rel_vel;
Vector3 temp1 = Vector3.TransformNormal(Vector3.Cross(relPosA, latVel), bodyA.InvInertiaTensorWorld);
Vector3 temp2 = Vector3.TransformNormal(Vector3.Cross(relPosB, latVel), bodyB.InvInertiaTensorWorld);
float friction_impulse = lat_rel_vel /
(bodyA.InverseMass + bodyB.InverseMass + Vector3.Dot(latVel, Vector3.Cross(temp1, relPosA) + Vector3.Cross(temp2, relPosB)));
float normal_impulse = cpd.AppliedImpulse * combinedFriction;
MathHelper.SetMin(ref friction_impulse, normal_impulse);
MathHelper.SetMin(ref friction_impulse, -normal_impulse);
bodyA.ApplyImpulse(latVel * -friction_impulse, relPosA);
bodyB.ApplyImpulse(latVel * friction_impulse, relPosB);
}
}
return normalImpulse;
}
/// Changes a btManifoldPoint collision normal to the normal from the mesh.
public static void AdjustInternalEdgeContacts(ManifoldPoint cp, CollisionObject colObj0, CollisionObject colObj1, int partId0, int index0, InternalEdgeAdjustFlags normalAdjustFlags)
{
//btAssert(colObj0.GetCollisionShape().GetShapeType() == TRIANGLE_SHAPE_PROXYTYPE);
if (colObj0.GetCollisionShape().GetShapeType() != BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE)
return;
BvhTriangleMeshShape trimesh = null;
if (colObj0.GetRootCollisionShape().GetShapeType() == BroadphaseNativeTypes.SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE)
{
//trimesh = ((ScaledBvhTriangleMeshShape)colObj0.GetRootCollisionShape()).GetChildShape();
}
else
{
trimesh = (BvhTriangleMeshShape)colObj0.GetRootCollisionShape();
}
TriangleInfoMap triangleInfoMapPtr = (TriangleInfoMap)trimesh.GetTriangleInfoMap();
if (triangleInfoMapPtr == null)
{
return;
}
int hash = GetHash(partId0, index0);
TriangleInfo info;
if (!triangleInfoMapPtr.TryGetValue(hash, out info))
{
return;
}
float frontFacing = (normalAdjustFlags & InternalEdgeAdjustFlags.BT_TRIANGLE_CONVEX_BACKFACE_MODE) == 0 ? 1.0f : -1.0f;
TriangleShape tri_shape = colObj0.GetCollisionShape() as TriangleShape;
IndexedVector3 v0, v1, v2;
tri_shape.GetVertex(0, out v0);
tri_shape.GetVertex(1, out v1);
tri_shape.GetVertex(2, out v2);
IndexedVector3 center = (v0 + v1 + v2) * (1.0f / 3.0f);
IndexedVector3 red = new IndexedVector3(1, 0, 0), green = new IndexedVector3(0, 1, 0), blue = new IndexedVector3(0, 0, 1), white = new IndexedVector3(1, 1, 1), black = new IndexedVector3(0, 0, 0);
IndexedVector3 tri_normal;
tri_shape.CalcNormal(out tri_normal);
//float dot = tri_normal.dot(cp.m_normalWorldOnB);
IndexedVector3 nearest;
NearestPointInLineSegment(ref cp.m_localPointB, ref v0, ref v1, out nearest);
IndexedVector3 contact = cp.m_localPointB;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
IndexedMatrix tr = colObj0.GetWorldTransform();
DebugDrawLine(tr * nearest, tr * cp.m_localPointB, red);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
bool isNearEdge = false;
int numConcaveEdgeHits = 0;
int numConvexEdgeHits = 0;
IndexedVector3 localContactNormalOnB = colObj0.GetWorldTransform()._basis.Transpose() * cp.m_normalWorldOnB;
localContactNormalOnB.Normalize();//is this necessary?
// Get closest edge
int bestedge = -1;
float disttobestedge = MathUtil.BT_LARGE_FLOAT;
//
// Edge 0 . 1
if (Math.Abs(info.m_edgeV0V1Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
{
//IndexedVector3 nearest;
NearestPointInLineSegment(ref cp.m_localPointB, ref v0, ref v1, out nearest);
float len = (contact - nearest).Length();
//
if (len < disttobestedge)
{
bestedge = 0;
disttobestedge = len;
}
}
// Edge 1 . 2
if (Math.Abs(info.m_edgeV1V2Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
{
//IndexedVector3 nearest;
NearestPointInLineSegment(ref cp.m_localPointB, ref v1, ref v2, out nearest);
float len = (contact - nearest).Length();
//
if (len < disttobestedge)
{
bestedge = 1;
disttobestedge = len;
}
}
// Edge 2 . 0
if (Math.Abs(info.m_edgeV2V0Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
{
//IndexedVector3 nearest;
NearestPointInLineSegment(ref cp.m_localPointB, ref v2, ref v0, out nearest);
float len = (contact - nearest).Length();
//
if (len < disttobestedge)
{
bestedge = 2;
disttobestedge = len;
}
}
#if BT_INTERNAL_EDGE_DEBUG_DRAW
IndexedVector3 upfix = tri_normal * new IndexedVector3(0.1f, 0.1f, 0.1f);
DebugDrawLine(tr * v0 + upfix, tr * v1 + upfix, red);
#endif
if (Math.Abs(info.m_edgeV0V1Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
{
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black);
#endif
float len = (contact - nearest).Length();
if (len < triangleInfoMapPtr.m_edgeDistanceThreshold)
if (bestedge == 0)
{
IndexedVector3 edge = (v0 - v1);
isNearEdge = true;
if (info.m_edgeV0V1Angle == 0.0f)
{
numConcaveEdgeHits++;
}
else
{
bool isEdgeConvex = (info.m_flags & TriangleInfoMap.TRI_INFO_V0V1_CONVEX) != 0;
float swapFactor = isEdgeConvex ? 1.0f : -1.0f;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
IndexedVector3 nA = swapFactor * tri_normal;
IndexedQuaternion orn = new IndexedQuaternion(edge, info.m_edgeV0V1Angle);
IndexedVector3 computedNormalB = MathUtil.QuatRotate(ref orn, ref tri_normal);
if ((info.m_flags & TriangleInfoMap.TRI_INFO_V0V1_SWAP_NORMALB) != 0)
{
computedNormalB *= -1;
}
IndexedVector3 nB = swapFactor * computedNormalB;
float NdotA = localContactNormalOnB.Dot(ref nA);
float NdotB = localContactNormalOnB.Dot(ref nB);
bool backFacingNormal = (NdotA < triangleInfoMapPtr.m_convexEpsilon) && (NdotB < triangleInfoMapPtr.m_convexEpsilon);
#if DEBUG_INTERNAL_EDGE
{
DebugDrawLine(cp.GetPositionWorldOnB(), cp.GetPositionWorldOnB() + tr._basis * (nB * 20), red);
}
#endif //DEBUG_INTERNAL_EDGE
if (backFacingNormal)
{
numConcaveEdgeHits++;
}
else
{
numConvexEdgeHits++;
IndexedVector3 clampedLocalNormal;
bool isClamped = ClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB, info.m_edgeV0V1Angle, out clampedLocalNormal);
if (isClamped)
{
if (((normalAdjustFlags & InternalEdgeAdjustFlags.BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.Dot(frontFacing * tri_normal) > 0))
{
IndexedVector3 newNormal = colObj0.GetWorldTransform()._basis * clampedLocalNormal;
// cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
cp.m_normalWorldOnB = newNormal;
// Reproject collision point along normal. (what about cp.m_distance1?)
cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
cp.m_localPointB = colObj0.GetWorldTransform().InvXform(cp.m_positionWorldOnB);
}
}
}
}
}
}
NearestPointInLineSegment(ref contact, ref v1, ref v2, out nearest);
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * nearest, tr * cp.m_localPointB, green);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * v1 + upfix, tr * v2 + upfix, green);
#endif
if (Math.Abs(info.m_edgeV1V2Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
{
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
float len = (contact - nearest).Length();
if (len < triangleInfoMapPtr.m_edgeDistanceThreshold)
if (bestedge == 1)
{
isNearEdge = true;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * nearest, tr * (nearest + tri_normal * 10), white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
IndexedVector3 edge = (v1 - v2);
isNearEdge = true;
if (info.m_edgeV1V2Angle == 0f)
{
numConcaveEdgeHits++;
}
else
{
bool isEdgeConvex = (info.m_flags & TriangleInfoMap.TRI_INFO_V1V2_CONVEX) != 0;
float swapFactor = isEdgeConvex ? 1.0f : -1.0f;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
IndexedVector3 nA = swapFactor * tri_normal;
IndexedQuaternion orn = new IndexedQuaternion(edge, info.m_edgeV1V2Angle);
IndexedVector3 computedNormalB = MathUtil.QuatRotate(ref orn, ref tri_normal);
if ((info.m_flags & TriangleInfoMap.TRI_INFO_V1V2_SWAP_NORMALB) != 0)
{
computedNormalB *= -1;
}
IndexedVector3 nB = swapFactor * computedNormalB;
#if DEBUG_INTERNAL_EDGE
{
DebugDrawLine(cp.GetPositionWorldOnB(), cp.GetPositionWorldOnB() + tr._basis * (nB * 20), red);
}
#endif //DEBUG_INTERNAL_EDGE
float NdotA = localContactNormalOnB.Dot(ref nA);
float NdotB = localContactNormalOnB.Dot(ref nB);
bool backFacingNormal = (NdotA < triangleInfoMapPtr.m_convexEpsilon) && (NdotB < triangleInfoMapPtr.m_convexEpsilon);
if (backFacingNormal)
{
numConcaveEdgeHits++;
}
else
{
numConvexEdgeHits++;
IndexedVector3 localContactNormalOnB2 = colObj0.GetWorldTransform()._basis.Transpose() * cp.m_normalWorldOnB;
IndexedVector3 clampedLocalNormal;
bool isClamped = ClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB2, info.m_edgeV1V2Angle, out clampedLocalNormal);
if (isClamped)
{
if (((normalAdjustFlags & InternalEdgeAdjustFlags.BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.Dot(frontFacing * tri_normal) > 0))
{
IndexedVector3 newNormal = colObj0.GetWorldTransform()._basis * clampedLocalNormal;
// cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
cp.m_normalWorldOnB = newNormal;
// Reproject collision point along normal.
cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
cp.m_localPointB = colObj0.GetWorldTransform().InvXform(cp.m_positionWorldOnB);
}
}
}
}
}
}
NearestPointInLineSegment(ref contact, ref v2, ref v0, out nearest);
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * nearest, tr * cp.m_localPointB, blue);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * v2 + upfix, tr * v0 + upfix, blue);
#endif
if (Math.Abs(info.m_edgeV2V0Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
{
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
float len = (contact - nearest).Length();
if (len < triangleInfoMapPtr.m_edgeDistanceThreshold)
if (bestedge == 2)
{
isNearEdge = true;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * nearest, tr * (nearest + tri_normal * 10), white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
IndexedVector3 edge = (v2 - v0);
if (info.m_edgeV2V0Angle == 0f)
{
numConcaveEdgeHits++;
}
else
{
bool isEdgeConvex = (info.m_flags & TriangleInfoMap.TRI_INFO_V2V0_CONVEX) != 0;
float swapFactor = isEdgeConvex ? 1.0f : -1.0f;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
IndexedVector3 nA = swapFactor * tri_normal;
IndexedQuaternion orn = new IndexedQuaternion(edge, info.m_edgeV2V0Angle);
IndexedVector3 computedNormalB = MathUtil.QuatRotate(ref orn, ref tri_normal);
if ((info.m_flags & TriangleInfoMap.TRI_INFO_V2V0_SWAP_NORMALB) != 0)
{
computedNormalB *= -1;
}
IndexedVector3 nB = swapFactor * computedNormalB;
#if DEBUG_INTERNAL_EDGE
{
DebugDrawLine(cp.GetPositionWorldOnB(), cp.GetPositionWorldOnB() + tr._basis * (nB * 20), red);
}
#endif //DEBUG_INTERNAL_EDGE
float NdotA = localContactNormalOnB.Dot(ref nA);
float NdotB = localContactNormalOnB.Dot(ref nB);
bool backFacingNormal = (NdotA < triangleInfoMapPtr.m_convexEpsilon) && (NdotB < triangleInfoMapPtr.m_convexEpsilon);
if (backFacingNormal)
{
numConcaveEdgeHits++;
}
else
{
numConvexEdgeHits++;
// printf("hitting convex edge\n");
IndexedVector3 localContactNormalOnB2 = colObj0.GetWorldTransform()._basis.Transpose() * cp.m_normalWorldOnB;
IndexedVector3 clampedLocalNormal;
bool isClamped = ClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB2, info.m_edgeV2V0Angle, out clampedLocalNormal);
if (isClamped)
{
if (((normalAdjustFlags & InternalEdgeAdjustFlags.BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.Dot(frontFacing * tri_normal) > 0))
{
IndexedVector3 newNormal = colObj0.GetWorldTransform()._basis * clampedLocalNormal;
// cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
cp.m_normalWorldOnB = newNormal;
// Reproject collision point along normal.
cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
cp.m_localPointB = colObj0.GetWorldTransform().InvXform(cp.m_positionWorldOnB);
}
}
}
}
}
}
#if DEBUG_INTERNAL_EDGE
{
IndexedVector3 color = new IndexedVector3(0, 1, 1);
DebugDrawLine(cp.GetPositionWorldOnB(), cp.GetPositionWorldOnB() + cp.m_normalWorldOnB * 10, color);
}
#endif //DEBUG_INTERNAL_EDGE
if (isNearEdge)
{
if (numConcaveEdgeHits > 0)
{
if ((normalAdjustFlags & InternalEdgeAdjustFlags.BT_TRIANGLE_CONCAVE_DOUBLE_SIDED) != 0)
{
//fix tri_normal so it pointing the same direction as the current local contact normal
if (tri_normal.Dot(ref localContactNormalOnB) < 0)
{
tri_normal *= -1;
}
cp.m_normalWorldOnB = colObj0.GetWorldTransform()._basis * tri_normal;
}
else
{
IndexedVector3 newNormal = tri_normal * frontFacing;
//if the tri_normal is pointing opposite direction as the current local contact normal, skip it
float d = newNormal.Dot(ref localContactNormalOnB);
if (d < 0)
{
return;
}
//modify the normal to be the triangle normal (or backfacing normal)
cp.m_normalWorldOnB = colObj0.GetWorldTransform()._basis * newNormal;
}
// Reproject collision point along normal.
cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
cp.m_localPointB = colObj0.GetWorldTransform().InvXform(cp.m_positionWorldOnB);
}
}
}
/// <summary>
/// contact constraint resolution:
/// calculate and apply impulse to satisfy non-penetration and non-negative relative velocity constraint
/// positive distance = separation, negative distance = penetration
/// </summary>
/// <param name="body1"></param>
/// <param name="body2"></param>
/// <param name="contactPoint"></param>
/// <param name="info"></param>
/// <returns></returns>
public static float ResolveSingleCollision(RigidBody bodyA, RigidBody bodyB,
ManifoldPoint contactPoint, ContactSolverInfo solverInfo)
{
Vector3 pos1 = contactPoint.PositionWorldOnA;
Vector3 pos2 = contactPoint.PositionWorldOnB;
// printf("distance=%f\n",distance);
Vector3 normal = contactPoint.NormalWorldOnB;
Vector3 rel_pos1 = pos1 - bodyA.CenterOfMassPosition;
Vector3 rel_pos2 = pos2 - bodyB.CenterOfMassPosition;
Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1);
Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2);
Vector3 vel = vel1 - vel2;
float rel_vel;
rel_vel = Vector3.Dot(normal, vel);
float Kfps = 1f / solverInfo.TimeStep;
//float damping = solverInfo.m_damping;
float Kerp = solverInfo.Erp;
float Kcor = Kerp * Kfps;
//printf("dist=%f\n",distance);
ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;
if (cpd == null)
throw new BulletException();
float distance = cpd.Penetration;//contactPoint.getDistance();
//distance = 0.f;
float positionalError = Kcor * -distance;
//jacDiagABInv;
float velocityError = cpd.Restitution - rel_vel;// * damping;
float penetrationImpulse = positionalError * cpd.JacDiagABInv;
float velocityImpulse = velocityError * cpd.JacDiagABInv;
float normalImpulse = penetrationImpulse + velocityImpulse;
// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
float oldNormalImpulse = cpd.AppliedImpulse;
float sum = oldNormalImpulse + normalImpulse;
cpd.AppliedImpulse = 0f > sum ? 0f : sum;
normalImpulse = cpd.AppliedImpulse - oldNormalImpulse;
if (bodyA.InverseMass != 0)
{
bodyA.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyA.InverseMass, cpd.AngularComponentA, normalImpulse);
}
if (bodyB.InverseMass != 0)
{
bodyB.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyB.InverseMass, cpd.AngularComponentB, -normalImpulse);
}
/*body1.applyImpulse(normal * (normalImpulse), rel_pos1);
body2.applyImpulse(-normal * (normalImpulse), rel_pos2);*/
return normalImpulse;
}
public static float ResolveSingleFriction(RigidBody bodyA, RigidBody bodyB,
ManifoldPoint contactPoint, ContactSolverInfo solverInfo)
{
Vector3 pos1 = contactPoint.PositionWorldOnA;
Vector3 pos2 = contactPoint.PositionWorldOnB;
Vector3 rel_pos1 = pos1 - bodyA.CenterOfMassPosition;
Vector3 rel_pos2 = pos2 - bodyB.CenterOfMassPosition;
ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;
if (cpd == null)
throw new BulletException();
float combinedFriction = cpd.Friction;
float limit = cpd.AppliedImpulse * combinedFriction;
//friction
if (cpd.AppliedImpulse > 0)
{
//apply friction in the 2 tangential directions
// 1st tangent
Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1);
Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2);
Vector3 vel = vel1 - vel2;
float j1, j2;
{
float vrel = Vector3.Dot(cpd.FrictionWorldTangentialA, vel);
// calculate j that moves us to zero relative velocity
j1 = -vrel * cpd.JacDiagABInvTangentA;
float oldTangentImpulse = cpd.AccumulatedTangentImpulseA;
cpd.AccumulatedTangentImpulseA = oldTangentImpulse + j1;
float atia = cpd.AccumulatedTangentImpulseA;
MathHelper.SetMin(ref atia, limit);
MathHelper.SetMax(ref atia, -limit);
cpd.AccumulatedTangentImpulseA = atia;
j1 = cpd.AccumulatedTangentImpulseA - oldTangentImpulse;
}
{
// 2nd tangent
float vrel = Vector3.Dot(cpd.FrictionWorldTangentialB, vel);
// calculate j that moves us to zero relative velocity
j2 = -vrel * cpd.JacDiagABInvTangentB;
float oldTangentImpulse = cpd.AccumulatedTangentImpulseB;
cpd.AccumulatedTangentImpulseB = oldTangentImpulse + j2;
float atib = cpd.AccumulatedTangentImpulseB;
MathHelper.SetMin(ref atib, limit);
MathHelper.SetMax(ref atib, -limit);
cpd.AccumulatedTangentImpulseB = atib;
j2 = cpd.AccumulatedTangentImpulseB - oldTangentImpulse;
}
if (bodyA.InverseMass != 0)
{
bodyA.InternalApplyImpulse(cpd.FrictionWorldTangentialA * bodyA.InverseMass, cpd.FrictionAngularComponent0A, j1);
bodyA.InternalApplyImpulse(cpd.FrictionWorldTangentialB * bodyA.InverseMass, cpd.FrictionAngularComponent1A, j2);
}
if (bodyB.InverseMass != 0)
{
bodyB.InternalApplyImpulse(cpd.FrictionWorldTangentialA * bodyB.InverseMass, cpd.FrictionAngularComponent0B, -j1);
bodyB.InternalApplyImpulse(cpd.FrictionWorldTangentialB * bodyB.InverseMass, cpd.FrictionAngularComponent1B, -j2);
}
}
return cpd.AppliedImpulse;
}
protected float SolveCombinedContactFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw)
{
float maxImpulse = 0;
Vector3 color = new Vector3(0, 1, 0);
if (cp.Distance <= 0)
{
if (iter == 0)
if (debugDraw != null)
debugDraw.DrawContactPoint(cp.PositionWorldOnB, cp.NormalWorldOnB, cp.Distance, cp.LifeTime, color);
float impulse = ContactConstraint.ResolveSingleCollisionCombined(
bodyA, bodyB,
cp,
info);
if (maxImpulse < impulse)
maxImpulse = impulse;
}
return maxImpulse;
}
protected float Solve(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw)
{
float maxImpulse = 0;
Vector3 color = new Vector3(0, 1, 0);
if (cp.Distance <= 0)
{
if (iter == 0)
if(debugDraw != null)
debugDraw.DrawContactPoint(cp.PositionWorldOnB, cp.NormalWorldOnB, cp.Distance, cp.LifeTime, color);
ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData;
float impulse = cpd.ContactSolverFunc(
bodyA, bodyB,
cp,
info);
if (maxImpulse < impulse)
maxImpulse = impulse;
}
return maxImpulse;
}
protected float Solve(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter)
{
return Solve(bodyA, bodyB, cp, info, iter, null);
}
