489 lines
17 KiB
C#
489 lines
17 KiB
C#
/*
|
|
Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
|
|
Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
|
|
|
|
This software is provided 'as-is', without any express or implied
|
|
warranty. In no event will the authors be held liable for any damages
|
|
arising from the use of this software.
|
|
|
|
Permission is granted to anyone to use this software for any purpose,
|
|
including commercial applications, and to alter it and redistribute it
|
|
freely, subject to the following restrictions:
|
|
|
|
1. The origin of this software must not be misrepresented; you must not
|
|
claim that you wrote the original software. If you use this software
|
|
in a product, an acknowledgment in the product documentation would be
|
|
appreciated but is not required.
|
|
2. Altered source versions must be plainly marked as such, and must not be
|
|
misrepresented as being the original software.
|
|
3. This notice may not be removed or altered from any source distribution.
|
|
*/
|
|
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Text;
|
|
using MonoXnaCompactMaths;
|
|
|
|
namespace XnaDevRu.BulletX.Dynamics
|
|
{
|
|
public delegate float ContactSolverFunc (RigidBody bodyA, RigidBody bodyB, ManifoldPoint contactPoint, ContactSolverInfo info);
|
|
|
|
public enum ContactSolverType
|
|
{
|
|
Default = 0,
|
|
TypeA,
|
|
TypeB,
|
|
User,
|
|
MaxContactSolverType,
|
|
}
|
|
|
|
public class ConstraintPersistentData
|
|
{
|
|
// total applied impulse during most recent frame
|
|
private float _appliedImpulse;
|
|
private float _previousAppliedImpulse;
|
|
private float _accumulatedTangentImpulse0;
|
|
private float _accumulatedTangentImpulse1;
|
|
|
|
private float _jacDiagABInv;
|
|
private float _jacDiagABInvTangentA;
|
|
private float _jacDiagABInvTangentB;
|
|
private int _persistentLifeTime;
|
|
private float _restitution;
|
|
private float _friction;
|
|
private float _penetration;
|
|
private Vector3 _frictionWorldTangentialA;
|
|
private Vector3 _frictionWorldTangentialB;
|
|
|
|
private Vector3 _frictionAngularComponent0A;
|
|
private Vector3 _frictionAngularComponent0B;
|
|
private Vector3 _frictionAngularComponent1A;
|
|
private Vector3 _frictionAngularComponent1B;
|
|
|
|
//some data doesn't need to be persistent over frames: todo: clean/reuse this
|
|
private Vector3 _angularComponentA;
|
|
private Vector3 _angularComponentB;
|
|
|
|
private ContactSolverFunc _contactSolverFunc;
|
|
private ContactSolverFunc _frictionSolverFunc;
|
|
|
|
public float AppliedImpulse { get { return _appliedImpulse; } set { _appliedImpulse = value; } }
|
|
public float PreviousAppliedImpulse { get { return _previousAppliedImpulse; } set { _previousAppliedImpulse = value; } }
|
|
public float AccumulatedTangentImpulseA { get { return _accumulatedTangentImpulse0; } set { _accumulatedTangentImpulse0 = value; } }
|
|
public float AccumulatedTangentImpulseB { get { return _accumulatedTangentImpulse1; } set { _accumulatedTangentImpulse1 = value; } }
|
|
|
|
public float JacDiagABInv { get { return _jacDiagABInv; } set { _jacDiagABInv = value; } }
|
|
public float JacDiagABInvTangentA { get { return _jacDiagABInvTangentA; } set { _jacDiagABInvTangentA = value; } }
|
|
public float JacDiagABInvTangentB { get { return _jacDiagABInvTangentB; } set { _jacDiagABInvTangentB = value; } }
|
|
public int PersistentLifeTime { get { return _persistentLifeTime; } set { _persistentLifeTime = value; } }
|
|
public float Restitution { get { return _restitution; } set { _restitution = value; } }
|
|
public float Friction { get { return _friction; } set { _friction = value; } }
|
|
public float Penetration { get { return _penetration; } set { _penetration = value; } }
|
|
public Vector3 FrictionWorldTangentialA { get { return _frictionWorldTangentialA; } set { _frictionWorldTangentialA = value; } }
|
|
public Vector3 FrictionWorldTangentialB { get { return _frictionWorldTangentialB; } set { _frictionWorldTangentialB = value; } }
|
|
|
|
public Vector3 FrictionAngularComponent0A { get { return _frictionAngularComponent0A; } set { _frictionAngularComponent0A = value; } }
|
|
public Vector3 FrictionAngularComponent0B { get { return _frictionAngularComponent0B; } set { _frictionAngularComponent0B = value; } }
|
|
public Vector3 FrictionAngularComponent1A { get { return _frictionAngularComponent1A; } set { _frictionAngularComponent1A = value; } }
|
|
public Vector3 FrictionAngularComponent1B { get { return _frictionAngularComponent1B; } set { _frictionAngularComponent1B = value; } }
|
|
|
|
public Vector3 AngularComponentA { get { return _angularComponentA; } set { _angularComponentA = value; } }
|
|
public Vector3 AngularComponentB { get { return _angularComponentB; } set { _angularComponentB = value; } }
|
|
|
|
public ContactSolverFunc ContactSolverFunc { get { return _contactSolverFunc; } set { _contactSolverFunc = value; } }
|
|
public ContactSolverFunc FrictionSolverFunc { get { return _frictionSolverFunc; } set { _frictionSolverFunc = value; } }
|
|
}
|
|
|
|
public static class ContactConstraint
|
|
{
|
|
private const int UseInternalApplyImpulse = 1;
|
|
|
|
/// <summary>
|
|
/// bilateral constraint between two dynamic objects
|
|
/// positive distance = separation, negative distance = penetration
|
|
/// </summary>
|
|
/// <param name="body1"></param>
|
|
/// <param name="pos1"></param>
|
|
/// <param name="body2"></param>
|
|
/// <param name="pos2"></param>
|
|
/// <param name="distance"></param>
|
|
/// <param name="normal"></param>
|
|
/// <param name="impulse"></param>
|
|
/// <param name="timeStep"></param>
|
|
public static void ResolveSingleBilateral(RigidBody bodyA, Vector3 posA,
|
|
RigidBody bodyB, Vector3 posB,
|
|
float distance, Vector3 normal, out float impulse, float timeStep)
|
|
{
|
|
float normalLenSqr = normal.LengthSquared();
|
|
|
|
if (Math.Abs(normalLenSqr) >= 1.1f)
|
|
throw new BulletException();
|
|
|
|
/*if (normalLenSqr > 1.1f)
|
|
{
|
|
impulse = 0f;
|
|
return;
|
|
}*/
|
|
Vector3 rel_pos1 = posA - bodyA.CenterOfMassPosition;
|
|
Vector3 rel_pos2 = posB - bodyB.CenterOfMassPosition;
|
|
//this jacobian entry could be re-used for all iterations
|
|
|
|
Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1);
|
|
Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2);
|
|
Vector3 vel = vel1 - vel2;
|
|
|
|
|
|
JacobianEntry jac = new JacobianEntry(Matrix.Transpose(bodyA.CenterOfMassTransform),
|
|
Matrix.Transpose(bodyB.CenterOfMassTransform),
|
|
rel_pos1, rel_pos2, normal, bodyA.InvInertiaDiagLocal, bodyA.InverseMass,
|
|
bodyB.InvInertiaDiagLocal, bodyB.InverseMass);
|
|
|
|
float jacDiagAB = jac.Diagonal;
|
|
float jacDiagABInv = 1f / jacDiagAB;
|
|
|
|
float rel_vel = jac.GetRelativeVelocity(
|
|
bodyA.LinearVelocity,
|
|
Vector3.TransformNormal(bodyA.AngularVelocity, Matrix.Transpose(bodyA.CenterOfMassTransform)),
|
|
bodyB.LinearVelocity,
|
|
Vector3.TransformNormal(bodyB.AngularVelocity, Matrix.Transpose(bodyB.CenterOfMassTransform)));
|
|
float a;
|
|
a = jacDiagABInv;
|
|
|
|
|
|
rel_vel = Vector3.Dot(normal, vel);
|
|
|
|
float contactDamping = 0.2f;
|
|
|
|
float velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
|
|
impulse = velocityImpulse;
|
|
}
|
|
|
|
|
|
/// <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;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
//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;
|
|
}
|
|
|
|
public static float ResolveSingleFrictionEmpty(
|
|
RigidBody bodyA,
|
|
RigidBody bodyB,
|
|
ManifoldPoint contactPoint,
|
|
ContactSolverInfo solverInfo)
|
|
{
|
|
return 0;
|
|
}
|
|
}
|
|
}
|