public void ConvexSweepTest(ConvexShape castShape, ref IndexedMatrix convexFromWorld, ref IndexedMatrix convexToWorld, ConvexResultCallback resultCallback, float allowedCcdPenetration) { IndexedMatrix convexFromTrans = convexFromWorld; IndexedMatrix convexToTrans = convexToWorld; IndexedVector3 castShapeAabbMin; IndexedVector3 castShapeAabbMax; /* Compute AABB that encompasses angular movement */ IndexedVector3 linVel, angVel; TransformUtil.CalculateVelocity(ref convexFromTrans, ref convexToTrans, 1.0f, out linVel, out angVel); // FIXME MAN check this - should be a get/set rotation call, basis copy like this may break with scale? IndexedMatrix R = IndexedMatrix.Identity; R.SetRotation(convexFromTrans.GetRotation()); castShape.CalculateTemporalAabb(ref R, ref linVel, ref angVel, 1.0f, out castShapeAabbMin, out castShapeAabbMax); /// go over all objects, and if the ray intersects their aabb + cast shape aabb, // do a ray-shape query using convexCaster (CCD) for (int i = 0; i < m_overlappingObjects.Count; i++) { CollisionObject collisionObject = m_overlappingObjects[i]; //only perform raycast if filterMask matches if (resultCallback.NeedsCollision(collisionObject.GetBroadphaseHandle())) { //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); IndexedVector3 collisionObjectAabbMin; IndexedVector3 collisionObjectAabbMax; IndexedMatrix t = collisionObject.GetWorldTransform(); collisionObject.GetCollisionShape().GetAabb(ref t, out collisionObjectAabbMin, out collisionObjectAabbMax); AabbUtil2.AabbExpand(ref collisionObjectAabbMin, ref collisionObjectAabbMax, ref castShapeAabbMin, ref castShapeAabbMax); float hitLambda = 1f; //could use resultCallback.m_closestHitFraction, but needs testing IndexedVector3 hitNormal; if (AabbUtil2.RayAabb(convexFromWorld._origin, convexToWorld._origin, ref collisionObjectAabbMin, ref collisionObjectAabbMax, ref hitLambda, out hitNormal)) { IndexedMatrix wt = collisionObject.GetWorldTransform(); CollisionWorld.ObjectQuerySingle(castShape, ref convexFromTrans, ref convexToTrans, collisionObject, collisionObject.GetCollisionShape(), ref wt, resultCallback, allowedCcdPenetration); } } } }
public virtual void RemoveCollisionObject(CollisionObject collisionObject) { //bool removeFromBroadphase = false; { BroadphaseProxy bp = collisionObject.GetBroadphaseHandle(); if (bp != null) { // // only clear the cached algorithms // GetBroadphase().GetOverlappingPairCache().CleanProxyFromPairs(bp, m_dispatcher1); GetBroadphase().DestroyProxy(bp, m_dispatcher1); collisionObject.SetBroadphaseHandle(null); } } //swapremove m_collisionObjects.Remove(collisionObject); }
public void UpdateSingleAabb(CollisionObject colObj) { IndexedVector3 minAabb; IndexedVector3 maxAabb; IndexedMatrix wt = colObj.GetWorldTransform(); colObj.GetCollisionShape().GetAabb(ref wt, out minAabb, out maxAabb); //need to increase the aabb for contact thresholds IndexedVector3 contactThreshold = new IndexedVector3(BulletGlobals.gContactBreakingThreshold); minAabb -= contactThreshold; maxAabb += contactThreshold; if (GetDispatchInfo().m_useContinuous && colObj.GetInternalType() == CollisionObjectTypes.CO_RIGID_BODY && !colObj.IsStaticOrKinematicObject()) { IndexedVector3 minAabb2,maxAabb2; colObj.GetCollisionShape().GetAabb(colObj.GetInterpolationWorldTransform(),out minAabb2 ,out maxAabb2); minAabb2 -= contactThreshold; maxAabb2 += contactThreshold; MathUtil.VectorMin(ref minAabb2,ref minAabb); MathUtil.VectorMax(ref maxAabb2, ref maxAabb); } if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugCollisionWorld) { MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "updateSingleAabbMin", minAabb); MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "updateSingleAabbMax", maxAabb); } IBroadphaseInterface bp = m_broadphasePairCache as IBroadphaseInterface; //moving objects should be moderately sized, probably something wrong if not if (colObj.IsStaticObject() || ((maxAabb - minAabb).LengthSquared() < 1e12f)) { bp.SetAabb(colObj.GetBroadphaseHandle(), ref minAabb, ref maxAabb, m_dispatcher1); } else { //something went wrong, investigate //this assert is unwanted in 3D modelers (danger of loosing work) colObj.SetActivationState(ActivationState.DISABLE_SIMULATION); //static bool reportMe = true; bool reportMe = true; if (reportMe && m_debugDrawer != null) { reportMe = false; m_debugDrawer.ReportErrorWarning("Overflow in AABB, object removed from simulation"); m_debugDrawer.ReportErrorWarning("If you can reproduce this, please email [email protected]\n"); m_debugDrawer.ReportErrorWarning("Please include above information, your Platform, version of OS.\n"); m_debugDrawer.ReportErrorWarning("Thanks.\n"); } } }