public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { CollisionObject convexbody = _isSwapped ? bodyB : bodyA; CollisionObject triBody = _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.InterpolationWorldTransform.Translation - convexbody.WorldTransform.Translation).LengthSquared(); if (squareMot0 < convexbody.CcdSquareMotionThreshold) { return(1); } Matrix triInv = MathHelper.InvertMatrix(triBody.WorldTransform); Matrix convexFromLocal = triInv * convexbody.WorldTransform; Matrix convexToLocal = triInv * convexbody.InterpolationWorldTransform; if (triBody.CollisionShape.IsConcave) { Vector3 rayAabbMin = convexFromLocal.Translation; MathHelper.SetMin(ref rayAabbMin, convexToLocal.Translation); Vector3 rayAabbMax = convexFromLocal.Translation; MathHelper.SetMax(ref rayAabbMax, convexToLocal.Translation); float ccdRadius0 = convexbody.CcdSweptSphereRadius; rayAabbMin -= new Vector3(ccdRadius0, ccdRadius0, ccdRadius0); rayAabbMax += new Vector3(ccdRadius0, ccdRadius0, ccdRadius0); float curHitFraction = 1f; //is this available? LocalTriangleSphereCastCallback raycastCallback = new LocalTriangleSphereCastCallback(convexFromLocal, convexToLocal, convexbody.CcdSweptSphereRadius, curHitFraction); raycastCallback.HitFraction = convexbody.HitFraction; CollisionObject concavebody = triBody; ConcaveShape triangleMesh = concavebody.CollisionShape as ConcaveShape; if (triangleMesh != null) { triangleMesh.ProcessAllTriangles(raycastCallback, rayAabbMin, rayAabbMax); } if (raycastCallback.HitFraction < convexbody.HitFraction) { convexbody.HitFraction = raycastCallback.HitFraction; return(raycastCallback.HitFraction); } } return(1); }
public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { CollisionObject convexbody = _isSwapped ? bodyB : bodyA; CollisionObject triBody = _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.InterpolationWorldTransform.Translation - convexbody.WorldTransform.Translation).LengthSquared(); if (squareMot0 < convexbody.CcdSquareMotionThreshold) { return 1; } Matrix triInv = MathHelper.InvertMatrix(triBody.WorldTransform); Matrix convexFromLocal = triInv * convexbody.WorldTransform; Matrix convexToLocal = triInv * convexbody.InterpolationWorldTransform; if (triBody.CollisionShape.IsConcave) { Vector3 rayAabbMin = convexFromLocal.Translation; MathHelper.SetMin(ref rayAabbMin, convexToLocal.Translation); Vector3 rayAabbMax = convexFromLocal.Translation; MathHelper.SetMax(ref rayAabbMax, convexToLocal.Translation); float ccdRadius0 = convexbody.CcdSweptSphereRadius; rayAabbMin -= new Vector3(ccdRadius0, ccdRadius0, ccdRadius0); rayAabbMax += new Vector3(ccdRadius0, ccdRadius0, ccdRadius0); float curHitFraction = 1f; //is this available? LocalTriangleSphereCastCallback raycastCallback = new LocalTriangleSphereCastCallback(convexFromLocal, convexToLocal, convexbody.CcdSweptSphereRadius, curHitFraction); raycastCallback.HitFraction = convexbody.HitFraction; CollisionObject concavebody = triBody; ConcaveShape triangleMesh = concavebody.CollisionShape as ConcaveShape; if (triangleMesh != null) { triangleMesh.ProcessAllTriangles(raycastCallback, rayAabbMin, rayAabbMax); } if (raycastCallback.HitFraction < convexbody.HitFraction) { convexbody.HitFraction = raycastCallback.HitFraction; return raycastCallback.HitFraction; } } return 1; }
internal override double calculateTimeOfImpact( btCollisionObject body0, btCollisionObject body1, btDispatcherInfo dispatchInfo, btManifoldResult resultOut ) { //(void)resultOut; //(void)dispatchInfo; btCollisionObject convexbody = m_isSwapped ? body1 : body0; btCollisionObject 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... double squareMot0 = btVector3.btDelLength2( ref convexbody.m_interpolationWorldTransform.m_origin, ref convexbody.m_worldTransform.m_origin ); if( squareMot0 < convexbody.getCcdSquareMotionThreshold() ) { return btScalar.BT_ONE; } //ref btVector3 from = convexbody.m_worldTransform.getOrigin(); //btVector3 to = convexbody.m_interpolationWorldTransform.getOrigin(); //todo: only do if the motion exceeds the 'radius' btTransform triInv; triBody.m_worldTransform.inverse( out triInv ); btTransform convexFromLocal; triInv.Apply( ref convexbody.m_worldTransform, out convexFromLocal ); btTransform convexToLocal; triInv.Apply( ref convexbody.m_interpolationWorldTransform, out convexToLocal ); if( triBody.getCollisionShape().isConcave() ) { btVector3 rayAabbMin = convexFromLocal.m_origin; rayAabbMin.setMin( ref convexToLocal.m_origin ); btVector3 rayAabbMax = convexFromLocal.m_origin; rayAabbMax.setMax( ref convexToLocal.m_origin ); double ccdRadius0 = convexbody.getCcdSweptSphereRadius(); rayAabbMin.AddScale( ref btVector3.One, -ccdRadius0, out rayAabbMin ); rayAabbMax.AddScale( ref btVector3.One, ccdRadius0, out rayAabbMax ); double curHitFraction = btScalar.BT_ONE; //is this available? LocalTriangleSphereCastCallback raycastCallback = new LocalTriangleSphereCastCallback ( ref convexFromLocal, ref convexToLocal, convexbody.getCcdSweptSphereRadius(), curHitFraction); raycastCallback.m_hitFraction = convexbody.getHitFraction(); btCollisionObject concavebody = triBody; btConcaveShape triangleMesh = (btConcaveShape)concavebody.getCollisionShape(); 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 btScalar.BT_ONE; }