public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
//resultOut = new ManifoldResult();
if (m_manifoldPtr == null)
{
return;
}
CollisionObject sphereObj = m_swapped? body1 : body0;
CollisionObject triObj = m_swapped? body0 : body1;
SphereShape sphere = (SphereShape)sphereObj.GetCollisionShape();
TriangleShape triangle = (TriangleShape)triObj.GetCollisionShape();
/// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut.SetPersistentManifold(m_manifoldPtr);
SphereTriangleDetector detector = new SphereTriangleDetector(sphere,triangle, m_manifoldPtr.GetContactBreakingThreshold());
ClosestPointInput input = new ClosestPointInput();
input.m_maximumDistanceSquared = float.MaxValue;
input.m_transformA = sphereObj.GetWorldTransform();
input.m_transformB = triObj.GetWorldTransform();
bool swapResults = m_swapped;
detector.GetClosestPoints(input,resultOut,dispatchInfo.getDebugDraw(),swapResults);
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
}
/// <summary>
///
/// </summary>
/// <returns></returns>
public DispatcherInfo RetriveDispatcherInfo()
{
InitilaizeBinding();
RetriveDispatcherInfoRequest wcfReq = new RetriveDispatcherInfoRequest();
RetriveDispatcherInfoResponse wcfRes = null;
var channelFactory = new ChannelFactory <IFwkService>(binding, address);
IFwkService client = null;
try
{
client = channelFactory.CreateChannel();
wcfRes = client.RetriveDispatcherInfo(wcfReq);
((ICommunicationObject)client).Close();
}
catch (Exception ex)
{
if (client != null)
{
((ICommunicationObject)client).Abort();
}
throw ex;
}
DispatcherInfo wDispatcherInfo = wcfRes.RetriveDispatcherInfoResult;
return(wDispatcherInfo);
}
public void NearCallback(BroadphasePair collisionPair, CollisionDispatcher dispatcher, DispatcherInfo dispatchInfo)
{
CollisionObject colObj0 = (CollisionObject)collisionPair.m_pProxy0.GetClientObject();
CollisionObject colObj1 = (CollisionObject)collisionPair.m_pProxy1.GetClientObject();
if (dispatcher.NeedsCollision(colObj0,colObj1))
{
//dispatcher will keep algorithms persistent in the collision pair
if (collisionPair.m_algorithm == null)
{
collisionPair.m_algorithm = dispatcher.FindAlgorithm(colObj0,colObj1,null);
}
if (collisionPair.m_algorithm != null)
{
ManifoldResult contactPointResult = new ManifoldResult(colObj0,colObj1);
if (dispatchInfo.GetDispatchFunc() == DispatchFunc.DISPATCH_DISCRETE)
{
//discrete collision detection query
collisionPair.m_algorithm.ProcessCollision(colObj0,colObj1,dispatchInfo,contactPointResult);
}
else
{
//continuous collision detection query, time of impact (toi)
float toi = collisionPair.m_algorithm.CalculateTimeOfImpact(colObj0,colObj1,dispatchInfo,contactPointResult);
if (dispatchInfo.GetTimeOfImpact() > toi)
{
dispatchInfo.SetTimeOfImpact(toi);
}
}
}
}
}
static void DispatcherNearCallback(BroadphasePair collisionPair, CollisionDispatcher dispatcher,
DispatcherInfo dispatchInfo)
{
//AddToDisposeQueue(dispatchInfo.DebugDraw);
TestManifoldPoints();
//Console.WriteLine("DispatcherNearCallback");
}
public override void StepSimulation(float timeStep, int numSubsteps, float fixedTimeStep)
{
//apply gravity, predict motion
PredictUnconstraintMotion(timeStep);
DispatcherInfo dispatchInfo = new DispatcherInfo();
dispatchInfo.TimeStep = timeStep;
dispatchInfo.StepCount = 0;
dispatchInfo.DebugDraw = DebugDrawer;
//perform collision detection
PerformDiscreteCollisionDetection();
//solve contact constraints
int numManifolds = Dispatcher.ManifoldCount;
if (numManifolds != 0)
{
List <PersistentManifold> manifolds = (Dispatcher as CollisionDispatcher).Manifolds;
//int numManifolds = m_dispatcher1.GetNumManifolds();
ContactSolverInfo infoGlobal = new ContactSolverInfo();
infoGlobal.TimeStep = timeStep;
_constraintSolver.SolveGroup(new List <CollisionObject>(), manifolds, manifolds.Count, new List <TypedConstraint>(), infoGlobal, _debugDrawer);
}
//integrate transforms
IntegrateTransforms(timeStep);
UpdateAabbs();
SynchronizeMotionStates();
}
//this constructor doesn't own the dispatcher and paircache/broadphase
public CollisionWorld(IDispatcher dispatcher,IBroadphaseInterface broadphasePairCache, ICollisionConfiguration collisionConfiguration)
{
m_dispatcher1 = dispatcher;
m_broadphasePairCache = broadphasePairCache;
m_collisionObjects = new ObjectArray<CollisionObject>();
m_dispatchInfo = new DispatcherInfo();
m_forceUpdateAllAabbs = true;
}
public override void ProcessCollision (CollisionObject body0,CollisionObject body1,DispatcherInfo dispatchInfo,ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
{
return;
}
CollisionObject convexObj = m_isSwapped? body1 : body0;
CollisionObject planeObj = m_isSwapped? body0: body1;
ConvexShape convexShape = (ConvexShape) convexObj.GetCollisionShape();
StaticPlaneShape planeShape = (StaticPlaneShape) planeObj.GetCollisionShape();
//bool hasCollision = false;
Vector3 planeNormal = planeShape.GetPlaneNormal();
//float planeConstant = planeShape.getPlaneConstant();
//first perform a collision query with the non-perturbated collision objects
{
Quaternion rotq = Quaternion.Identity;
CollideSingleContact(ref rotq,body0,body1,dispatchInfo,resultOut);
}
if (resultOut.GetPersistentManifold().GetNumContacts()<m_minimumPointsPerturbationThreshold)
{
Vector3 v0 = Vector3.Zero;
Vector3 v1 = Vector3.Zero;
TransformUtil.PlaneSpace1(ref planeNormal,ref v0,ref v1);
//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
float angleLimit = 0.125f * MathUtil.SIMD_PI;
float perturbeAngle;
float radius = convexShape.GetAngularMotionDisc();
perturbeAngle = BulletGlobals.gContactBreakingThreshold / radius;
if ( perturbeAngle > angleLimit )
{
perturbeAngle = angleLimit;
}
Quaternion perturbeRot = Quaternion.CreateFromAxisAngle(v0,perturbeAngle);
for (int i=0;i<m_numPerturbationIterations;i++)
{
float iterationAngle = i*(MathUtil.SIMD_2_PI/(float)m_numPerturbationIterations);
Quaternion rotq = Quaternion.CreateFromAxisAngle(planeNormal,iterationAngle);
rotq = MathUtil.QuaternionMultiply(Quaternion.Inverse(rotq),MathUtil.QuaternionMultiply(perturbeRot,rotq));
CollideSingleContact(ref rotq,body0,body1,dispatchInfo,resultOut);
}
}
if (m_ownManifold)
{
if (m_manifoldPtr.GetNumContacts() > 0)
{
resultOut.RefreshContactPoints();
}
}
}
public override void ProcessCollision (CollisionObject body0,CollisionObject body1,DispatcherInfo dispatchInfo,ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
{
//swapped?
m_manifoldPtr = m_dispatcher.GetNewManifold(body0,body1);
m_ownManifold = true;
}
resultOut.SetPersistentManifold(m_manifoldPtr);
//comment-out next line to test multi-contact generation
//resultOut.getPersistentManifold().clearManifold();
ConvexShape min0 = (ConvexShape)(body0.GetCollisionShape());
ConvexShape min1 = (ConvexShape)(body1.GetCollisionShape());
Vector3 normalOnB = Vector3.Zero;
Vector3 pointOnBWorld = Vector3.Zero;
{
ClosestPointInput input = new ClosestPointInput();
GjkPairDetector gjkPairDetector = new GjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver);
//TODO: if (dispatchInfo.m_useContinuous)
gjkPairDetector.SetMinkowskiA(min0);
gjkPairDetector.SetMinkowskiB(min1);
{
input.m_maximumDistanceSquared = min0.Margin + min1.Margin + m_manifoldPtr.GetContactBreakingThreshold();
input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
}
input.m_transformA = body0.GetWorldTransform();
input.m_transformB = body1.GetWorldTransform();
gjkPairDetector.GetClosestPoints(input,resultOut,dispatchInfo.getDebugDraw(),false);
if (BulletGlobals.g_streamWriter != null)
{
BulletGlobals.g_streamWriter.WriteLine("c2dc2d processCollision");
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "transformA", input.m_transformA);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "transformB", input.m_transformB);
}
//btVector3 v0,v1;
//btVector3 sepNormalWorldSpace;
}
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
}
public CompoundLeafCallback (CollisionObject compoundObj,CollisionObject otherObj,IDispatcher dispatcher,DispatcherInfo dispatchInfo,ManifoldResult resultOut,IList<CollisionAlgorithm> childCollisionAlgorithms,PersistentManifold sharedManifold)
{
m_compoundColObj = compoundObj;
m_otherObj = otherObj;
m_dispatcher = dispatcher;
m_dispatchInfo = dispatchInfo;
m_resultOut = resultOut;
m_childCollisionAlgorithms = childCollisionAlgorithms;
m_sharedManifold = sharedManifold;
}
public override void ProcessCollision (CollisionObject body0,CollisionObject body1,DispatcherInfo dispatchInfo,ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
{
return;
}
resultOut.SetPersistentManifold(m_manifoldPtr);
SphereShape sphere0 = (SphereShape)body0.GetCollisionShape();
SphereShape sphere1 = (SphereShape)body1.GetCollisionShape();
Vector3 diff = body0.GetWorldTransform().Translation - body1.GetWorldTransform().Translation;
float len = diff.Length();
float radius0 = sphere0.GetRadius();
float radius1 = sphere1.GetRadius();
#if CLEAR_MANIFOLD
m_manifoldPtr.clearManifold(); //don't do this, it disables warmstarting
#endif
///iff distance positive, don't generate a new contact
if ( len > (radius0+radius1))
{
#if !CLEAR_MANIFOLD
resultOut.RefreshContactPoints();
#endif //CLEAR_MANIFOLD
return;
}
///distance (negative means penetration)
float dist = len - (radius0+radius1);
Vector3 normalOnSurfaceB = new Vector3(1,0,0);
if (len > MathUtil.SIMD_EPSILON)
{
normalOnSurfaceB = diff / len;
}
///point on A (worldspace)
///btVector3 pos0 = col0->getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB;
///point on B (worldspace)
Vector3 pos1 = body1.GetWorldTransform().Translation + radius1* normalOnSurfaceB;
/// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut.AddContactPoint(ref normalOnSurfaceB,ref pos1,dist);
#if !CLEAR_MANIFOLD
resultOut.RefreshContactPoints();
#endif //CLEAR_MANIFOLD
}
protected virtual void InternalSingleStepSimulation(float timeStep)
{
BulletGlobals.StartProfile("internalSingleStepSimulation");
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugDiscreteDynamicsWorld)
{
BulletGlobals.g_streamWriter.WriteLine("internalSingleStepSimulation");
}
#endif
if (null != m_internalPreTickCallback)
{
m_internalPreTickCallback.InternalTickCallback(this, timeStep);
}
///apply gravity, predict motion
PredictUnconstraintMotion(timeStep);
DispatcherInfo dispatchInfo = GetDispatchInfo();
dispatchInfo.SetTimeStep(timeStep);
dispatchInfo.SetStepCount(0);
dispatchInfo.SetDebugDraw(GetDebugDrawer());
///perform collision detection
PerformDiscreteCollisionDetection();
CalculateSimulationIslands();
GetSolverInfo().m_timeStep = timeStep;
///solve contact and other joint constraints
SolveConstraints(GetSolverInfo());
///CallbackTriggers();
///integrate transforms
IntegrateTransforms(timeStep);
///update vehicle simulation
UpdateActions(timeStep);
UpdateActivationState(timeStep);
if (m_internalTickCallback != null)
{
m_internalTickCallback.InternalTickCallback(this, timeStep);
}
BulletGlobals.StopProfile();
}
/// <summary>
/// Initializes a new instance of the AzureDispatcher class
/// </summary>
/// <param name="info">indicating the dispatcher info</param>
/// <param name="binding">binding information</param>
/// <param name="sharedData">indicating the shared data</param>
/// <param name="observer">indicating the observer</param>
/// <param name="queueFactory">indicating the queue factory</param>
/// <param name="dispatcherIdle">set when the dispatcher enters idle status</param>
public AzureDispatcher(DispatcherInfo info, Binding binding, SharedData sharedData, BrokerObserver observer, BrokerQueueFactory queueFactory, SchedulerAdapterClientFactory schedulerAdapterClientFactory, AutoResetEvent dispatcherIdle)
: base(info, ProxyBinding.BrokerProxyBinding, sharedData, observer, queueFactory, schedulerAdapterClientFactory, dispatcherIdle)
{
// Initialize proxy client pool
this.proxyClientPool = this.AttachProxyClientPool(this.Epr, sharedData.Config.LoadBalancing.MaxConnectionCountPerAzureProxy);
// Update backend binding's maxMessageSize settings with global maxMessageSize if its enabled (> 0)
int maxMessageSize = sharedData.ServiceConfig.MaxMessageSize;
if (maxMessageSize > 0)
{
BindingHelper.ApplyMaxMessageSize(binding, maxMessageSize);
}
this.backendBindingData = new BindingData(binding);
}
public void SetTimeStepAndCounters(float collisionMarginTriangle, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
m_dispatchInfoPtr = dispatchInfo;
m_collisionMarginTriangle = collisionMarginTriangle;
m_resultOut = resultOut;
//recalc aabbs
//Matrix convexInTriangleSpace = MathUtil.bulletMatrixMultiply(Matrix.Invert(m_triBody.getWorldTransform()) , m_convexBody.getWorldTransform());
Matrix convexInTriangleSpace = MathUtil.InverseTimes(m_triBody.GetWorldTransform(), m_convexBody.GetWorldTransform());
CollisionShape convexShape = m_convexBody.GetCollisionShape();
convexShape.GetAabb(ref convexInTriangleSpace,ref m_aabbMin,ref m_aabbMax);
float extraMargin = collisionMarginTriangle;
Vector3 extra = new Vector3(extraMargin,extraMargin,extraMargin);
m_aabbMax += extra;
m_aabbMin -= extra;
}
/// <summary>
/// Retorna uyn alista de Service Metadata providers configurados en el Web Service Dispatcher
/// </summary>
/// <returns></returns>
public DispatcherInfo RetriveDispatcherInfo()
{
using (Singleservice.SingleService wService = new Singleservice.SingleService())
{
if (_Proxy != null)
{
wService.Proxy = _Proxy;
}
if (_Credentials != null)
{
wService.Credentials = _Credentials;
}
wService.Url = _URL;
Fwk.Bases.Connector.Singleservice.DispatcherInfo wsDispatcherInfo = wService.RetriveDispatcherInfo();
DispatcherInfo wDispatcherInfo = new DispatcherInfo();
wDispatcherInfo.ServiceDispatcherConnection = wsDispatcherInfo.ServiceDispatcherConnection;
wDispatcherInfo.MachineIp = wsDispatcherInfo.MachineIp;
wDispatcherInfo.ServiceDispatcherName = wsDispatcherInfo.ServiceDispatcherName;
List <Fwk.ConfigSection.MetadataProvider> providers = new List <MetadataProvider>();
MetadataProvider provider = null;
foreach (var p in wsDispatcherInfo.MetadataProviders)
{
provider = new MetadataProvider();
provider.ApplicationId = p.ApplicationId;
provider.ConfigProviderType = p.ConfigProviderType;
//provider.DefaultCulture = p.DefaultCulture;
provider.Name = p.Name;
provider.SecurityProviderName = p.SecurityProviderName;
provider.SourceInfo = p.SourceInfo;
wDispatcherInfo.MetadataProviders.Add(provider);
}
foreach (var p in wsDispatcherInfo.AppSettings)
{
wDispatcherInfo.AppSettings.Add(new DictionarySetting(p.Key, p.Value));
}
foreach (var p in wsDispatcherInfo.CnnStringSettings)
{
wDispatcherInfo.CnnStringSettings.Add(new DictionarySetting(p.Key, p.Value));
}
return(wDispatcherInfo);
}
}
public override void ProcessCollision (CollisionObject body0,CollisionObject body1,DispatcherInfo dispatchInfo,ManifoldResult resultOut)
{
//(void)dispatchInfo;
//(void)resultOut;
if (m_manifoldPtr == null)
{
resultOut = null;
return;
}
CollisionObject sphereObj = m_isSwapped? body1 : body0;
CollisionObject boxObj = m_isSwapped? body0 : body1;
SphereShape sphere0 = (SphereShape)sphereObj.GetCollisionShape();
//Vector3 normalOnSurfaceB;
Vector3 pOnBox = Vector3.Zero, pOnSphere = Vector3.Zero;
Vector3 sphereCenter = sphereObj.GetWorldTransform().Translation;
float radius = sphere0.GetRadius();
float dist = GetSphereDistance(boxObj,ref pOnBox,ref pOnSphere,ref sphereCenter,radius);
resultOut = new ManifoldResult();
resultOut.SetPersistentManifold(m_manifoldPtr);
if (dist < MathUtil.SIMD_EPSILON)
{
Vector3 normalOnSurfaceB = (pOnBox - pOnSphere);
normalOnSurfaceB.Normalize();
/// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut.AddContactPoint(ref normalOnSurfaceB,ref pOnBox,dist);
}
if (m_ownManifold)
{
if (m_manifoldPtr.GetNumContacts() > 0)
{
resultOut.RefreshContactPoints();
}
}
}
public void AddDispatcher(string dispatcherId, EndpointConnectionInfo info)
{
// all public functions should have locks since we concurrent access
lock (lockObj)
{
using (Trace.Log($"dispatcherId: {dispatcherId}, info: {info}"))
{
if (!dispatchers.TryGetValue(dispatcherId, out var dispatcher))
{
dispatcher = new DispatcherInfo
{
id = dispatcherId,
info = info
};
dispatchers[dispatcherId] = dispatcher;
}
}
}
}
public override void ProcessCollision (CollisionObject body0,CollisionObject body1,DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
{
return;
}
CollisionObject col0 = body0;
CollisionObject col1 = body1;
resultOut = new ManifoldResult(body0, body1);
BoxShape box0 = (BoxShape)col0.GetCollisionShape();
BoxShape box1 = (BoxShape)col1.GetCollisionShape();
//if (((String)col0.getUserPointer()).Contains("Box") &&
// ((String)col1.getUserPointer()).Contains("Box") )
//{
// int ibreak = 0;
//}
/// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut.SetPersistentManifold(m_manifoldPtr);
#if !USE_PERSISTENT_CONTACTS
m_manifoldPtr.ClearManifold();
#endif //USE_PERSISTENT_CONTACTS
ClosestPointInput input = new ClosestPointInput();
input.m_maximumDistanceSquared = float.MaxValue;
input.m_transformA = body0.GetWorldTransform();
input.m_transformB = body1.GetWorldTransform();
BoxBoxDetector detector = new BoxBoxDetector(box0,box1);
detector.GetClosestPoints(input,resultOut,dispatchInfo.getDebugDraw(),false);
#if USE_PERSISTENT_CONTACTS
// refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
#endif //USE_PERSISTENT_CONTACTS
}
/// <summary>
///
/// </summary>
/// <returns></returns>
public static DispatcherInfo RetriveDispatcherInfo()
{
DispatcherInfo dispatcherInfo = new DispatcherInfo();
try
{
dispatcherInfo.MachineIp = Fwk.HelperFunctions.EnvironmentFunctions.GetMachineIp();
}
catch (Exception e)
{
dispatcherInfo.MachineIp = e.Message;
}
List <MetadataProvider> list = new List <MetadataProvider>();
foreach (ServiceProviderElement providerElement in ServiceMetadata.ProviderSection.Providers)
{
list.Add(new MetadataProvider(providerElement));
}
dispatcherInfo.MetadataProviders = list;
dispatcherInfo.ServiceDispatcherConnection = System.Configuration.ConfigurationManager.AppSettings["ServiceDispatcherConnection"];
dispatcherInfo.ServiceDispatcherName = System.Configuration.ConfigurationManager.AppSettings["ServiceDispatcherName"];
foreach (string key in System.Configuration.ConfigurationManager.AppSettings)
{
dispatcherInfo.AppSettings.Add(key, System.Configuration.ConfigurationManager.AppSettings[key.ToString()].ToString());
}
foreach (System.Configuration.ConnectionStringSettings cnnStringSetting in System.Configuration.ConfigurationManager.ConnectionStrings)
{
dispatcherInfo.CnnStringSettings.Add(cnnStringSetting.Name, cnnStringSetting.ConnectionString);
}
MembershipSection wMembershipSection = (MembershipSection)System.Configuration.ConfigurationManager.GetSection("system.web/membership");
return(dispatcherInfo);
}
///maxSubSteps/fixedTimeStep for interpolation is currently ignored for btSimpleDynamicsWorld, use btDiscreteDynamicsWorld instead
public override int StepSimulation(float timeStep, int maxSubSteps, float fixedTimeStep)
{
///apply gravity, predict motion
PredictUnconstraintMotion(timeStep);
DispatcherInfo dispatchInfo = GetDispatchInfo();
dispatchInfo.SetTimeStep(timeStep);
dispatchInfo.SetStepCount(0);
dispatchInfo.SetDebugDraw(GetDebugDrawer());
///perform collision detection
PerformDiscreteCollisionDetection();
///solve contact constraints
int numManifolds = m_dispatcher1.GetNumManifolds();
if (numManifolds != 0)
{
PersistentManifoldArray manifoldPtr = (m_dispatcher1 as CollisionDispatcher).GetInternalManifoldPointer();
ContactSolverInfo infoGlobal = new ContactSolverInfo();
infoGlobal.m_timeStep = timeStep;
m_constraintSolver.PrepareSolve(0, numManifolds);
m_constraintSolver.SolveGroup(null, 0, manifoldPtr, 0, numManifolds, null, 0, 0, infoGlobal, m_debugDrawer, m_dispatcher1);
m_constraintSolver.AllSolved(infoGlobal, m_debugDrawer);
}
///integrate transforms
IntegrateTransforms(timeStep);
UpdateAabbs();
SynchronizeMotionStates();
ClearForces();
return(1);
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
{
return;
}
CollisionObject col0 = body0;
CollisionObject col1 = body1;
Box2dShape box0 = (Box2dShape)col0.GetCollisionShape();
Box2dShape box1 = (Box2dShape)col1.GetCollisionShape();
resultOut.SetPersistentManifold(m_manifoldPtr);
B2CollidePolygons(ref resultOut, box0, col0.GetWorldTransform(), box1, col1.GetWorldTransform());
// refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
}
protected virtual void InternalSingleStepSimulation(float timeStep)
{
StartProfiling(timeStep);
//update aabbs information
UpdateAabbs();
//apply gravity, predict motion
PredictUnconstraintMotion(timeStep);
DispatcherInfo dispatchInfo = DispatchInfo;
dispatchInfo.TimeStep = timeStep;
dispatchInfo.StepCount = 0;
dispatchInfo.DebugDraw = DebugDrawer;
//perform collision detection
PerformDiscreteCollisionDetection();
CalculateSimulationIslands();
SolverInfo.TimeStep = timeStep;
//solve contact and other joint constraints
SolveConstraints(SolverInfo);
//CallbackTriggers();
//integrate transforms
IntegrateTransforms(timeStep);
//update vehicle simulation
UpdateVehicles(timeStep);
UpdateActivationState(timeStep);
}
/// <summary>
/// Initializes a new instance of the AzureHttpsDispatcher class.
/// </summary>
/// <param name="azureQueueManager">AzureQueueManager instance</param>
/// <param name="info">indicating the dispatcher info</param>
/// <param name="binding">binding information</param>
/// <param name="sharedData">indicating the shared data</param>
/// <param name="observer">indicating the observer</param>
/// <param name="queueFactory">indicating the queue factory</param>
/// <param name="schedulerAdapterClientFactory">SchedulerAdapterClientFactory instance</param>
/// <param name="dispatcherIdle">set when the dispatcher enters idle status</param>
public AzureHttpsDispatcher(AzureQueueManager azureQueueManager, DispatcherInfo info, Binding binding, SharedData sharedData, BrokerObserver observer, BrokerQueueFactory queueFactory, SchedulerAdapterClientFactory schedulerAdapterClientFactory, AutoResetEvent dispatcherIdle)
: base(info, ProxyBinding.BrokerProxyBinding, sharedData, observer, queueFactory, schedulerAdapterClientFactory, dispatcherIdle)
{
AzureDispatcherInfo azureDispatcherInfo = info as AzureDispatcherInfo;
this.azureServiceName = azureDispatcherInfo.AzureServiceName;
this.azureQueueManager = azureQueueManager;
this.azureQueueManager.CreateRequestStorage(this.azureServiceName);
this.responseStorageName =
this.azureQueueManager.Start(azureDispatcherInfo.JobId, azureDispatcherInfo.RequeueCount);
// Update backend binding's maxMessageSize settings with global maxMessageSize if its enabled (> 0)
int maxMessageSize = sharedData.ServiceConfig.MaxMessageSize;
if (maxMessageSize > 0)
{
BindingHelper.ApplyMaxMessageSize(binding, maxMessageSize);
}
this.backendBindingData = new BindingData(binding);
}
public override float CalculateTimeOfImpact(CollisionObject body0,CollisionObject body1,DispatcherInfo dispatchInfo,ManifoldResult resultOut)
{
resultOut = null;
CollisionObject colObj = m_isSwapped? body1 : body0;
CollisionObject otherObj = m_isSwapped? body0 : body1;
Debug.Assert(colObj.GetCollisionShape().IsCompound());
CompoundShape compoundShape = (CompoundShape)(colObj.GetCollisionShape());
//We will use the OptimizedBVH, AABB tree to cull potential child-overlaps
//If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals
//given Proxy0 and Proxy1, if both have a tree, Tree0 and Tree1, this means:
//determine overlapping nodes of Proxy1 using Proxy0 AABB against Tree1
//then use each overlapping node AABB against Tree0
//and vise versa.
float hitFraction = 1f;
int numChildren = m_childCollisionAlgorithms.Count;
for (int i=0;i<numChildren;i++)
{
//temporarily exchange parent btCollisionShape with childShape, and recurse
CollisionShape childShape = compoundShape.GetChildShape(i);
//backup
Matrix orgTrans = colObj.GetWorldTransform();
Matrix childTrans = compoundShape.GetChildTransform(i);
Matrix newChildWorldTrans = MathUtil.BulletMatrixMultiply(ref orgTrans,ref childTrans);
colObj.SetWorldTransform(ref newChildWorldTrans);
CollisionShape tmpShape = colObj.GetCollisionShape();
colObj.InternalSetTemporaryCollisionShape( childShape );
float frac = m_childCollisionAlgorithms[i].CalculateTimeOfImpact(colObj,otherObj,dispatchInfo, resultOut);
if (frac<hitFraction)
{
hitFraction = frac;
}
//revert back
colObj.InternalSetTemporaryCollisionShape( tmpShape);
colObj.SetWorldTransform(ref orgTrans);
}
return hitFraction;
}
public override float CalculateTimeOfImpact(CollisionObject body0,CollisionObject body1,DispatcherInfo dispatchInfo,ManifoldResult resultOut)
{
///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
///body0.m_worldTransform,
float resultFraction = 1.0f;
float squareMot0 = (body0.GetInterpolationWorldTransform().Translation - body0.GetWorldTransform().Translation).LengthSquared();
float squareMot1 = (body1.GetInterpolationWorldTransform().Translation - body1.GetWorldTransform().Translation).LengthSquared();
if (squareMot0 < body0.GetCcdSquareMotionThreshold() &&
squareMot1 < body1.GetCcdSquareMotionThreshold())
{
return resultFraction;
}
//An adhoc way of testing the Continuous Collision Detection algorithms
//One object is approximated as a sphere, to simplify things
//Starting in penetration should report no time of impact
//For proper CCD, better accuracy and handling of 'allowed' penetration should be added
//also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
/// Convex0 against sphere for Convex1
{
ConvexShape convex0 = (ConvexShape)(body0.GetCollisionShape());
SphereShape sphere1 = new SphereShape(body1.GetCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
CastResult result = new CastResult();
VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver();
//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
///Simplification, one object is simplified as a sphere
GjkConvexCast ccd1 = new GjkConvexCast( convex0 ,sphere1,voronoiSimplex);
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
if (ccd1.CalcTimeOfImpact(body0.GetWorldTransform(),body0.GetInterpolationWorldTransform(),
body1.GetWorldTransform(),body1.GetInterpolationWorldTransform(),result))
{
//store result.m_fraction in both bodies
if (body0.GetHitFraction()> result.m_fraction)
{
body0.SetHitFraction( result.m_fraction );
}
if (body1.GetHitFraction() > result.m_fraction)
{
body1.SetHitFraction( result.m_fraction);
}
if (resultFraction > result.m_fraction)
{
resultFraction = result.m_fraction;
}
}
}
/// Sphere (for convex0) against Convex1
{
ConvexShape convex1 = (ConvexShape)(body1.GetCollisionShape());
SphereShape sphere0 = new SphereShape(body0.GetCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
CastResult result = new CastResult();
VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver();
//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
///Simplification, one object is simplified as a sphere
GjkConvexCast ccd1 = new GjkConvexCast(sphere0,convex1,voronoiSimplex);
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
if (ccd1.CalcTimeOfImpact(body0.GetWorldTransform(),body0.GetInterpolationWorldTransform(),
body1.GetWorldTransform(),body1.GetInterpolationWorldTransform(),result))
{
//store result.m_fraction in both bodies
if (body0.GetHitFraction() > result.m_fraction)
{
body0.SetHitFraction( result.m_fraction);
}
if (body1.GetHitFraction() > result.m_fraction)
{
body1.SetHitFraction( result.m_fraction);
}
if (resultFraction > result.m_fraction)
{
resultFraction = result.m_fraction;
}
}
}
return resultFraction;
}
/// <summary>
///
/// </summary>
/// <returns></returns>
public static DispatcherInfo RetriveDispatcherInfo()
{
DispatcherInfo dispatcherInfo = new DispatcherInfo();
try
{
dispatcherInfo.MachineIp = Fwk.HelperFunctions.EnvironmentFunctions.GetMachineIp();
}
catch (Exception e)
{
dispatcherInfo.MachineIp = e.Message;
}
List<MetadataProvider> list = new List<MetadataProvider>();
foreach (ServiceProviderElement providerElement in ServiceMetadata.ProviderSection.Providers)
{
list.Add(new MetadataProvider(providerElement));
}
dispatcherInfo.MetadataProviders = list;
dispatcherInfo.ServiceDispatcherConnection = System.Configuration.ConfigurationManager.AppSettings["ServiceDispatcherConnection"];
dispatcherInfo.ServiceDispatcherName = System.Configuration.ConfigurationManager.AppSettings["ServiceDispatcherName"];
foreach (string key in System.Configuration.ConfigurationManager.AppSettings)
{
dispatcherInfo.AppSettings.Add(key, System.Configuration.ConfigurationManager.AppSettings[key.ToString()].ToString());
}
foreach (System.Configuration.ConnectionStringSettings cnnStringSetting in System.Configuration.ConfigurationManager.ConnectionStrings)
{
dispatcherInfo.CnnStringSettings.Add(cnnStringSetting.Name, cnnStringSetting.ConnectionString);
}
MembershipSection wMembershipSection = (MembershipSection)System.Configuration.ConfigurationManager.GetSection("system.web/membership");
return dispatcherInfo;
}
public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
CollisionObject convexbody = m_isSwapped ? bodyB : bodyA;
CollisionObject triBody = m_isSwapped ? bodyA : bodyB;
//quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
//only perform CCD above a certain threshold, this prevents blocking on the long run
//because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
float squareMot0 = (convexbody.GetInterpolationWorldTransform().Translation - convexbody.GetWorldTransform().Translation).LengthSquared();
if (squareMot0 < convexbody.GetCcdSquareMotionThreshold())
{
return 1;
}
//Matrix triInv = MathHelper.InvertMatrix(triBody.getWorldTransform());
Matrix triInv = Matrix.Invert(triBody.GetWorldTransform());
Matrix convexFromLocal = triInv * convexbody.GetWorldTransform();
Matrix convexToLocal = triInv * convexbody.GetInterpolationWorldTransform();
if (triBody.GetCollisionShape().IsConcave())
{
Vector3 rayAabbMin = convexFromLocal.Translation;
MathUtil.VectorMin(convexToLocal.Translation ,ref rayAabbMin);
Vector3 rayAabbMax = convexFromLocal.Translation;
MathUtil.VectorMax(convexToLocal.Translation,ref rayAabbMax);
float ccdRadius0 = convexbody.GetCcdSweptSphereRadius();
rayAabbMin -= new Vector3(ccdRadius0, ccdRadius0, ccdRadius0);
rayAabbMax += new Vector3(ccdRadius0, ccdRadius0, ccdRadius0);
float curHitFraction = 1f; //is this available?
LocalTriangleSphereCastCallback raycastCallback = new LocalTriangleSphereCastCallback(ref convexFromLocal, ref convexToLocal,
convexbody.GetCcdSweptSphereRadius(), curHitFraction);
raycastCallback.m_hitFraction = convexbody.GetHitFraction();
CollisionObject concavebody = triBody;
ConcaveShape triangleMesh = concavebody.GetCollisionShape() as ConcaveShape;
if (triangleMesh != null)
{
triangleMesh.ProcessAllTriangles(raycastCallback, ref rayAabbMin, ref rayAabbMax);
}
if (raycastCallback.m_hitFraction < convexbody.GetHitFraction())
{
convexbody.SetHitFraction(raycastCallback.m_hitFraction);
return raycastCallback.m_hitFraction;
}
}
return 1;
}
public virtual void CollideSingleContact(ref Quaternion perturbeRot, CollisionObject body0,CollisionObject body1,DispatcherInfo dispatchInfo,ManifoldResult resultOut)
{
CollisionObject convexObj = m_isSwapped? body1 : body0;
CollisionObject planeObj = m_isSwapped? body0: body1;
ConvexShape convexShape = (ConvexShape) convexObj.GetCollisionShape();
StaticPlaneShape planeShape = (StaticPlaneShape) planeObj.GetCollisionShape();
bool hasCollision = false;
Vector3 planeNormal = planeShape.GetPlaneNormal();
float planeConstant = planeShape.GetPlaneConstant();
Matrix convexWorldTransform = convexObj.GetWorldTransform();
Matrix convexInPlaneTrans = Matrix.Identity;
convexInPlaneTrans = MathUtil.BulletMatrixMultiply(Matrix.Invert(planeObj.GetWorldTransform()), convexWorldTransform);
//now perturbe the convex-world transform
// MAN - CHECKTHIS
Matrix rotMatrix = Matrix.CreateFromQuaternion(perturbeRot);
convexWorldTransform = MathUtil.BulletMatrixMultiplyBasis(convexWorldTransform,rotMatrix);
Matrix planeInConvex = Matrix.Identity;
planeInConvex= MathUtil.BulletMatrixMultiply(Matrix.Invert(convexWorldTransform),planeObj.GetWorldTransform());
Vector3 tmp = Vector3.TransformNormal(-planeNormal,planeInConvex);
Vector3 vtx = convexShape.LocalGetSupportingVertex(ref tmp);
Vector3 vtxInPlane = Vector3.Transform(vtx,convexInPlaneTrans);
float distance = (Vector3.Dot(planeNormal,vtxInPlane) - planeConstant);
Vector3 vtxInPlaneProjected = vtxInPlane - (distance*planeNormal);
Vector3 vtxInPlaneWorld = Vector3.Transform(vtxInPlaneProjected,planeObj.GetWorldTransform());
hasCollision = distance < m_manifoldPtr.GetContactBreakingThreshold();
resultOut.SetPersistentManifold(m_manifoldPtr);
if (hasCollision)
{
/// report a contact. internally this will be kept persistent, and contact reduction is done
Vector3 normalOnSurfaceB = Vector3.TransformNormal(planeNormal,planeObj.GetWorldTransform());
Vector3 pOnB = vtxInPlaneWorld;
resultOut.AddContactPoint(ref normalOnSurfaceB,ref pOnB,distance);
}
}
public CollisionPairCallback(DispatcherInfo dispatchInfo,CollisionDispatcher dispatcher)
{
m_dispatchInfo = dispatchInfo;
m_dispatcher = dispatcher;
}
public override void ProcessCollision(CollisionObject body0,CollisionObject body1,DispatcherInfo dispatchInfo,ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
{
//swapped?
m_manifoldPtr = m_dispatcher.GetNewManifold(body0,body1);
m_ownManifold = true;
}
//resultOut = new ManifoldResult();
resultOut.SetPersistentManifold(m_manifoldPtr);
//comment-out next line to test multi-contact generation
//resultOut.getPersistentManifold().clearManifold();
ConvexShape min0 = (ConvexShape)(body0.GetCollisionShape());
ConvexShape min1 = (ConvexShape)(body1.GetCollisionShape());
Vector3 normalOnB = Vector3.Up;
Vector3 pointOnBWorld = Vector3.Zero;
#if !BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
if ((min0.ShapeType == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE) && (min1.ShapeType == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE))
{
CapsuleShape capsuleA = (CapsuleShape) min0;
CapsuleShape capsuleB = (CapsuleShape) min1;
Vector3 localScalingA = capsuleA.GetLocalScaling();
Vector3 localScalingB = capsuleB.GetLocalScaling();
float threshold = m_manifoldPtr.GetContactBreakingThreshold();
float dist = CapsuleCapsuleDistance(ref normalOnB,ref pointOnBWorld,capsuleA.getHalfHeight(),capsuleA.getRadius(),
capsuleB.getHalfHeight(),capsuleB.getRadius(),capsuleA.GetUpAxis(),capsuleB.GetUpAxis(),
body0.GetWorldTransform(),body1.GetWorldTransform(),threshold);
if (dist<threshold)
{
Debug.Assert(normalOnB.LengthSquared() >= (MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON));
resultOut.AddContactPoint(ref normalOnB,ref pointOnBWorld,dist);
}
resultOut.RefreshContactPoints();
return;
}
#endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
m_sepDistance.updateSeparatingDistance(body0.getWorldTransform(),body1.getWorldTransform());
}
if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0.f)
#endif //USE_SEPDISTANCE_UTIL2
{
ClosestPointInput input = new ClosestPointInput();
GjkPairDetector gjkPairDetector = new GjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver);
//TODO: if (dispatchInfo.m_useContinuous)
gjkPairDetector.SetMinkowskiA(min0);
gjkPairDetector.SetMinkowskiB(min1);
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
input.m_maximumDistanceSquared = float.MaxValue;
}
else
#endif //USE_SEPDISTANCE_UTIL2
{
input.m_maximumDistanceSquared = min0.Margin + min1.Margin + m_manifoldPtr.GetContactBreakingThreshold();
input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
}
//input.m_stackAlloc = dispatchInfo.m_stackAllocator;
input.m_transformA = body0.GetWorldTransform();
input.m_transformB = body1.GetWorldTransform();
gjkPairDetector.GetClosestPoints(input,resultOut,dispatchInfo.getDebugDraw(),false);
#if USE_SEPDISTANCE_UTIL2
float sepDist = 0.f;
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
sepDist = gjkPairDetector.getCachedSeparatingDistance();
if (sepDist>MathUtil.SIMD_EPSILON)
{
sepDist += dispatchInfo.m_convexConservativeDistanceThreshold;
//now perturbe directions to get multiple contact points
}
}
#endif //USE_SEPDISTANCE_UTIL2
//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
//perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
if (m_numPerturbationIterations > 0 && resultOut.GetPersistentManifold().GetNumContacts() < m_minimumPointsPerturbationThreshold)
{
Vector3 v0 = Vector3.Zero, v1 = Vector3.Zero;
Vector3 sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis();
sepNormalWorldSpace.Normalize();
TransformUtil.PlaneSpace1(ref sepNormalWorldSpace, ref v0, ref v1);
bool perturbeA = true;
const float angleLimit = 0.125f * MathUtil.SIMD_PI;
float perturbeAngle;
float radiusA = min0.GetAngularMotionDisc();
float radiusB = min1.GetAngularMotionDisc();
if (radiusA < radiusB)
{
perturbeAngle = BulletGlobals.gContactBreakingThreshold /radiusA;
perturbeA = true;
}
else
{
perturbeAngle = BulletGlobals.gContactBreakingThreshold / radiusB;
perturbeA = false;
}
if (perturbeAngle > angleLimit)
{
perturbeAngle = angleLimit;
}
Matrix unPerturbedTransform = Matrix.Identity;
if (perturbeA)
{
unPerturbedTransform = input.m_transformA;
}
else
{
unPerturbedTransform = input.m_transformB;
}
for (int i=0;i<m_numPerturbationIterations;i++)
{
if (v0.LengthSquared() > MathUtil.SIMD_EPSILON)
{
Quaternion perturbeRot = Quaternion.CreateFromAxisAngle(v0, perturbeAngle);
float iterationAngle = i * (MathUtil.SIMD_2_PI / (float)m_numPerturbationIterations);
Quaternion rotq = Quaternion.CreateFromAxisAngle(sepNormalWorldSpace, iterationAngle);
if (perturbeA)
{
//input.m_transformA.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0.getWorldTransform().getBasis());
Quaternion temp = MathUtil.QuaternionMultiply(MathUtil.QuaternionInverse(ref rotq),MathUtil.QuaternionMultiply(perturbeRot,rotq));
input.m_transformA = MathUtil.BulletMatrixMultiplyBasis(Matrix.CreateFromQuaternion(temp),body0.GetWorldTransform());
input.m_transformB = body1.GetWorldTransform();
#if DEBUG_CONTACTS
dispatchInfo.m_debugDraw.DrawTransform(ref input.m_transformA,10.0f);
#endif //DEBUG_CONTACTS
}
else
{
input.m_transformA = body0.GetWorldTransform();
//input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1.getWorldTransform().getBasis());
Quaternion temp = MathUtil.QuaternionMultiply(MathUtil.QuaternionInverse(ref rotq),MathUtil.QuaternionMultiply(perturbeRot,rotq));
input.m_transformB = MathUtil.BulletMatrixMultiplyBasis(Matrix.CreateFromQuaternion(temp),body1.GetWorldTransform());
#if DEBUG_CONTACTS
dispatchInfo.m_debugDraw.DrawTransform(ref input.m_transformB,10.0f);
#endif
}
PerturbedContactResult perturbedResultOut = new PerturbedContactResult(resultOut, ref input.m_transformA, ref input.m_transformB, ref unPerturbedTransform, perturbeA, dispatchInfo.getDebugDraw());
gjkPairDetector.GetClosestPoints(input, perturbedResultOut, dispatchInfo.getDebugDraw(), false);
}
}
}
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist > MathUtil.SIMD_EPSILON))
{
m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0.getWorldTransform(),body1.getWorldTransform());
}
#endif //USE_SEPDISTANCE_UTIL2
}
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
}
public override void ProcessCollision (CollisionObject body0,CollisionObject body1,DispatcherInfo dispatchInfo,ManifoldResult resultOut)
{
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
//resultOut = null;
CollisionObject colObj = m_isSwapped? body1 : body0;
CollisionObject otherObj = m_isSwapped? body0 : body1;
System.Diagnostics.Debug.Assert(colObj.GetCollisionShape().IsCompound());
CompoundShape compoundShape = (CompoundShape)(colObj.GetCollisionShape());
///btCompoundShape might have changed:
////make sure the internal child collision algorithm caches are still valid
if (compoundShape.GetUpdateRevision() != m_compoundShapeRevision)
{
///clear and update all
RemoveChildAlgorithms();
PreallocateChildAlgorithms(body0,body1);
}
Dbvt tree = compoundShape.GetDynamicAabbTree();
//use a dynamic aabb tree to cull potential child-overlaps
CompoundLeafCallback callback = new CompoundLeafCallback(colObj,otherObj,m_dispatcher,dispatchInfo,resultOut,m_childCollisionAlgorithms,m_sharedManifold);
///we need to refresh all contact manifolds
///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep
///so we should add a 'refreshManifolds' in the btCollisionAlgorithm
{
IList<PersistentManifold> manifoldArray = new List<PersistentManifold>();
for (int i=0;i<m_childCollisionAlgorithms.Count;i++)
{
if (m_childCollisionAlgorithms[i] != null)
{
m_childCollisionAlgorithms[i].GetAllContactManifolds(manifoldArray);
for (int m=0;m<manifoldArray.Count;m++)
{
if (manifoldArray[m].GetNumContacts() > 0)
{
resultOut.SetPersistentManifold(manifoldArray[m]);
resultOut.RefreshContactPoints();
resultOut.SetPersistentManifold(null);//??necessary?
}
}
manifoldArray.Clear();
}
}
}
if (tree != null)
{
Vector3 localAabbMin = new Vector3();
Vector3 localAabbMax = new Vector3();
Matrix otherInCompoundSpace = Matrix.Identity;
//otherInCompoundSpace = MathUtil.BulletMatrixMultiply(colObj.GetWorldTransform(),otherObj.GetWorldTransform());
otherInCompoundSpace = MathUtil.InverseTimes(colObj.GetWorldTransform(), otherObj.GetWorldTransform());
otherObj.GetCollisionShape().GetAabb(ref otherInCompoundSpace,ref localAabbMin,ref localAabbMax);
DbvtAabbMm bounds = DbvtAabbMm.FromMM(ref localAabbMin, ref localAabbMax);
//process all children, that overlap with the given AABB bounds
Dbvt.CollideTV(tree.m_root,ref bounds,callback);
}
else
{
//iterate over all children, perform an AABB check inside ProcessChildShape
int numChildren = m_childCollisionAlgorithms.Count;
for (int i=0;i<numChildren;i++)
{
callback.ProcessChildShape(compoundShape.GetChildShape(i),i);
}
}
{
//iterate over all children, perform an AABB check inside ProcessChildShape
int numChildren = m_childCollisionAlgorithms.Count;
IList<PersistentManifold> manifoldArray = new List<PersistentManifold>();
for (int i=0;i<numChildren;i++)
{
if (m_childCollisionAlgorithms[i] != null)
{
CollisionShape childShape = compoundShape.GetChildShape(i);
//if not longer overlapping, remove the algorithm
Matrix orgTrans = colObj.GetWorldTransform();
Matrix orgInterpolationTrans = colObj.GetInterpolationWorldTransform();
Matrix childTrans = compoundShape.GetChildTransform(i);
Matrix newChildWorldTrans = MathUtil.BulletMatrixMultiply(ref orgTrans, ref childTrans);
//perform an AABB check first
Vector3 aabbMin0 = new Vector3();
Vector3 aabbMax0 = new Vector3();
Vector3 aabbMin1 = new Vector3();
Vector3 aabbMax1 = new Vector3();
childShape.GetAabb(ref newChildWorldTrans,ref aabbMin0,ref aabbMax0);
otherObj.GetCollisionShape().GetAabb(otherObj.GetWorldTransform(),ref aabbMin1,ref aabbMax1);
if (!AabbUtil2.TestAabbAgainstAabb2(ref aabbMin0,ref aabbMax0,ref aabbMin1,ref aabbMax1))
{
m_dispatcher.FreeCollisionAlgorithm(m_childCollisionAlgorithms[i]);
m_childCollisionAlgorithms[i] = null;
}
}
}
}
}
//public override void processCollision (CollisionObject body0,CollisionObject body1,DispatcherInfo dispatchInfo,ManifoldResult resultOut)
//{
// CollisionObject convexBody = m_isSwapped ? body1 : body0;
// CollisionObject triBody = m_isSwapped ? body0 : body1;
// if (triBody.getCollisionShape().isConcave())
// {
// CollisionObject triOb = triBody;
// ConcaveShape concaveShape = (ConcaveShape)(triOb.getCollisionShape());
// if (convexBody.getCollisionShape().isConvex())
// {
// float collisionMarginTriangle = concaveShape.getMargin();
// resultOut.setPersistentManifold(m_convexTriangleCallback.m_manifoldPtr);
// m_convexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, resultOut);
// //Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here.
// //m_dispatcher.clearManifold(m_convexTriangleCallback.m_manifoldPtr);
// m_convexTriangleCallback.m_manifoldPtr.setBodies(convexBody, triBody);
// Vector3 min = m_convexTriangleCallback.getAabbMin();
// Vector3 max = m_convexTriangleCallback.getAabbMax();
// concaveShape.processAllTriangles(m_convexTriangleCallback, ref min,ref max );
// resultOut.refreshContactPoints();
// }
// }
//}
//public override float calculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
//{
// CollisionObject convexbody = m_isSwapped ? body1 : body0;
// CollisionObject triBody = m_isSwapped ? body0 : body1;
// //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
// //only perform CCD above a certain threshold, this prevents blocking on the long run
// //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
// float squareMot0 = (convexbody.getInterpolationWorldTransform().Translation - convexbody.getWorldTransform().Translation).LengthSquared();
// if (squareMot0 < convexbody.getCcdSquareMotionThreshold())
// {
// return 1f;
// }
// //const Vector3& from = convexbody.m_worldTransform.Translation;
// //Vector3 to = convexbody.m_interpolationWorldTransform.Translation;
// //todo: only do if the motion exceeds the 'radius'
// //Matrix triInv = Matrix.Invert(triBody.getWorldTransform());
// //Matrix convexFromLocal = MathUtil.bulletMatrixMultiply(triInv , convexbody.getWorldTransform());
// //Matrix convexToLocal = MathUtil.bulletMatrixMultiply(triInv , convexbody.getInterpolationWorldTransform());
// Matrix triInv = Matrix.Invert(triBody.getWorldTransform());
// Matrix convexFromLocal = MathUtil.inverseTimes(triBody.getWorldTransform(), convexbody.getWorldTransform());
// Matrix convexToLocal = MathUtil.inverseTimes(triBody.getWorldTransform(), convexbody.getInterpolationWorldTransform());
// if (triBody.getCollisionShape().isConcave())
// {
// Vector3 rayAabbMin = convexFromLocal.Translation;
// MathUtil.vectorMin(convexToLocal.Translation, ref rayAabbMin);
// Vector3 rayAabbMax = convexFromLocal.Translation;
// MathUtil.vectorMax(convexToLocal.Translation,ref rayAabbMax);
// float ccdRadius0 = convexbody.getCcdSweptSphereRadius();
// rayAabbMin -= new Vector3(ccdRadius0,ccdRadius0,ccdRadius0);
// rayAabbMax += new Vector3(ccdRadius0,ccdRadius0,ccdRadius0);
// float curHitFraction = 1.0f; //is this available?
// LocalTriangleSphereCastCallback raycastCallback = new LocalTriangleSphereCastCallback(ref convexFromLocal, ref convexToLocal,
// convexbody.getCcdSweptSphereRadius(),curHitFraction);
// raycastCallback.m_hitFraction = convexbody.getHitFraction();
// CollisionObject concavebody = triBody;
// ConcaveShape triangleMesh = (ConcaveShape) concavebody.getCollisionShape();
// if (triangleMesh != null)
// {
// triangleMesh.processAllTriangles(raycastCallback,ref rayAabbMin,ref rayAabbMax);
// }
// if (raycastCallback.m_hitFraction < convexbody.getHitFraction())
// {
// convexbody.setHitFraction( raycastCallback.m_hitFraction);
// float result = raycastCallback.m_hitFraction;
// raycastCallback.cleanup();
// return result;
// }
// raycastCallback.cleanup();
// }
// return 1f;
//}
public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
//fixme
CollisionObject convexBody = m_isSwapped ? bodyB : bodyA;
CollisionObject triBody = m_isSwapped ? bodyA : bodyB;
if (triBody.GetCollisionShape().IsConcave())
{
CollisionObject triOb = triBody;
ConcaveShape concaveShape = triOb.GetCollisionShape() as ConcaveShape;
if (convexBody.GetCollisionShape().IsConvex())
{
float collisionMarginTriangle = concaveShape.Margin;
resultOut.SetPersistentManifold(m_convexTriangleCallback.m_manifoldPtr);
m_convexTriangleCallback.SetTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, resultOut);
//Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here.
//m_dispatcher->clearManifold(m_btConvexTriangleCallback.m_manifoldPtr);
m_convexTriangleCallback.m_manifoldPtr.SetBodies(convexBody, triBody);
Vector3 min = m_convexTriangleCallback.GetAabbMin();
Vector3 max = m_convexTriangleCallback.GetAabbMax();
concaveShape.ProcessAllTriangles(m_convexTriangleCallback, ref min,ref max);
resultOut.RefreshContactPoints();
}
}
}
public override void StepSimulation(float timeStep, int numSubsteps, float fixedTimeStep)
{
//apply gravity, predict motion
PredictUnconstraintMotion(timeStep);
DispatcherInfo dispatchInfo = new DispatcherInfo();
dispatchInfo.TimeStep = timeStep;
dispatchInfo.StepCount = 0;
dispatchInfo.DebugDraw = DebugDrawer;
//perform collision detection
PerformDiscreteCollisionDetection();
//solve contact constraints
int numManifolds = Dispatcher.ManifoldCount;
if (numManifolds != 0)
{
List<PersistentManifold> manifolds = (Dispatcher as CollisionDispatcher).Manifolds;
//int numManifolds = m_dispatcher1.GetNumManifolds();
ContactSolverInfo infoGlobal = new ContactSolverInfo();
infoGlobal.TimeStep = timeStep;
_constraintSolver.SolveGroup(new List<CollisionObject>(), manifolds, manifolds.Count, new List<TypedConstraint>(), infoGlobal, _debugDrawer);
}
//integrate transforms
IntegrateTransforms(timeStep);
UpdateAabbs();
SynchronizeMotionStates();
}
public override float CalculateTimeOfImpact(CollisionObject body0,CollisionObject body1,DispatcherInfo dispatchInfo,ManifoldResult resultOut)
{
resultOut = new ManifoldResult();
//not yet
return 1f;
}
public static void NearCallback(BroadphasePair collisionPair, CollisionDispatcher dispatcher, DispatcherInfo dispatcherInfo)
{
var entity1 = (collisionPair.Proxy0.ClientObject as RigidBody).UserObject as Entity;
var entity2 = (collisionPair.Proxy1.ClientObject as RigidBody).UserObject as Entity;
if (HardCollisionAction != null)
{
if (entity1.CollisionType == CollisionTypeEnum.Hard)
{
HardCollisionAction();
}
}
CollisionDispatcher.DefaultNearCallback(collisionPair, dispatcher, dispatcherInfo);
}
