public static void Run() { { btTransform planeObjWorld = new btTransform( new btQuaternion( 0.2, 0.1, 0.3, 1 ), new btVector3( 3, 4, 5 ) ); btTransform convexWorldTransform = new btTransform( new btQuaternion( 0.2, 0.3, 0, 1 ), new btVector3( 3, 4, 5 ) ); btTransform convexInPlaneTrans; btTransform tmp; planeObjWorld.inverse( out tmp ); tmp.Apply( ref convexWorldTransform, out convexInPlaneTrans ); PrintTransform( ref convexInPlaneTrans ); planeObjWorld.inverseTimes( ref convexWorldTransform, out convexInPlaneTrans ); PrintTransform( ref convexInPlaneTrans ); //convexInPlaneTrans = planeObjWorld( convexWorldTransform; //PrintTransform( &convexInPlaneTrans ); } { btTransform planeObjWorld = new btTransform( new btQuaternion( 0.2, 0.1, 0.3, 1 ), new btVector3( 5, 2, 1 ) ); btTransform convexWorldTransform = new btTransform( new btQuaternion( 0.2, 0.3, 0, 1 ), new btVector3( 3, 4, 5 ) ); btTransform convexInPlaneTrans; btTransform tmp; planeObjWorld.inverse( out tmp ); tmp.Apply( ref convexWorldTransform, out convexInPlaneTrans ); PrintTransform( ref convexInPlaneTrans ); planeObjWorld.inverseTimes( ref convexWorldTransform, out convexInPlaneTrans ); PrintTransform( ref convexInPlaneTrans ); //convexInPlaneTrans = planeObjWorld( convexWorldTransform; //PrintTransform( &convexInPlaneTrans ); btQuaternion perturbeRot = new btQuaternion( 0.1, 0.5, 0.25, 0.8 ); perturbeRot.normalize(); btTransform planeObjWrapTrans = new btTransform( new btQuaternion( 0.2, 0.1, 0.3, 1 ), new btVector3( 4, 7, 2 ) ); planeObjWrapTrans.inverseTimes( ref convexWorldTransform, out convexInPlaneTrans ); //now perturbe the convex-world transform btMatrix3x3 perturbeMat = new btMatrix3x3( ref perturbeRot ); btMatrix3x3 tmpPerturbe; convexWorldTransform.m_basis.Apply( ref perturbeMat, out tmpPerturbe ); convexWorldTransform.m_basis = tmpPerturbe; btTransform planeInConvex; convexWorldTransform.inverseTimes( ref planeObjWrapTrans, out planeInConvex ); PrintTransform( ref planeInConvex ); } Console.Read(); }
/// This maximum should not be necessary. It allows for untested/degenerate cases in production code. /// You don't want your game ever to lock-up. void computeClosestPoints( ref btTransform transA, ref btTransform transB, btPointCollector pointCollector) { if( m_convexB1 != null) { m_simplexSolver.reset(); btGjkPairDetector gjk = BulletGlobals.GjkPairDetectorPool.Get(); gjk.Initialize( m_convexA, m_convexB1, m_convexA.getShapeType(), m_convexB1.getShapeType(), m_convexA.getMargin(), m_convexB1.getMargin(), m_simplexSolver, m_penetrationDepthSolver); btGjkPairDetector.ClosestPointInput input = new btDiscreteCollisionDetectorInterface.ClosestPointInput(); input.m_transformA = transA; input.m_transformB = transB; gjk.getClosestPoints( input, pointCollector, null ); BulletGlobals.GjkPairDetectorPool.Free( gjk ); } else { //convex versus plane btConvexShape convexShape = m_convexA; btStaticPlaneShape planeShape = m_planeShape; btVector3 planeNormal; planeShape.getPlaneNormal( out planeNormal ); double planeConstant = planeShape.getPlaneConstant(); //btTransform convexWorldTransform = transA; btTransform convexInPlaneTrans; btTransform tmpInv; transB.inverse( out tmpInv ); tmpInv.Apply( ref transA, out convexInPlaneTrans ); btTransform planeInConvex; convexInPlaneTrans.inverse( out tmpInv ); tmpInv.Apply( ref transB, out planeInConvex ); //planeInConvex = convexWorldTransform.inverse() * transB; btVector3 tmp; planeInConvex.m_basis.Mult( ref planeNormal, out tmp ); tmp.Invert( out tmp ); btVector3 vtx; convexShape.localGetSupportingVertex( ref tmp, out vtx ); btVector3 vtxInPlane; convexInPlaneTrans.Apply( ref vtx, out vtxInPlane ); double distance = ( planeNormal.dot( vtxInPlane ) - planeConstant ); btVector3 vtxInPlaneProjected;// = vtxInPlane - distance * planeNormal; vtxInPlane.SubScale( ref planeNormal, distance, out vtxInPlaneProjected ); btVector3 vtxInPlaneWorld; transB.Apply(ref vtxInPlaneProjected, out vtxInPlaneWorld ); btVector3 normalOnSurfaceB; transB.m_basis.Apply( ref planeNormal, out normalOnSurfaceB ); pointCollector.addContactPoint( ref normalOnSurfaceB, ref vtxInPlaneWorld, distance ); } }
public static void objectQuerySingleInternal( btConvexShape castShape, ref btTransform convexFromTrans, ref btTransform convexToTrans , btCollisionShape collisionShape//btCollisionObjectWrapper colObjWrap , btCollisionObject collisionObject , ref btTransform colObjTransform , ConvexResultCallback resultCallback, double allowedPenetration ) { //btCollisionShape collisionShape = colObjWrap.getCollisionShape(); //btTransform colObjWorldTransform = colObjWrap.m_worldTransform; if( collisionShape.isConvex() ) { //CProfileSample sample = new CProfileSample("convexSweepConvex"); btConvexCast.CastResult castResult = new btConvexCast.CastResult(); castResult.m_allowedPenetration = allowedPenetration; castResult.m_fraction = resultCallback.m_closestHitFraction;//btScalar.BT_ONE;//?? btConvexShape convexShape = (btConvexShape)collisionShape; btVoronoiSimplexSolver simplexSolver = BulletGlobals.VoronoiSimplexSolverPool.Get(); btGjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver = BulletGlobals.GjkEpaPenetrationDepthSolverPool.Get(); // new btGjkEpaPenetrationDepthSolver(); btContinuousConvexCollision convexCaster1 = BulletGlobals.ContinuousConvexCollisionPool.Get(); convexCaster1.Initialize( castShape, convexShape, simplexSolver, gjkEpaPenetrationSolver ); //btGjkConvexCast convexCaster2(castShape,convexShape,&simplexSolver); //btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver); btConvexCast castPtr = convexCaster1; if( castPtr.calcTimeOfImpact( ref convexFromTrans, ref convexToTrans, ref colObjTransform, ref colObjTransform, castResult ) ) { //add hit if( castResult.m_normal.length2() > (double)( 0.0001 ) ) { if( castResult.m_fraction < resultCallback.m_closestHitFraction ) { castResult.m_normal.normalize(); LocalConvexResult localConvexResult = new LocalConvexResult ( collisionObject, null, ref castResult.m_normal, ref castResult.m_hitPoint, castResult.m_fraction ); bool normalInWorldSpace = true; resultCallback.addSingleResult( ref localConvexResult, normalInWorldSpace ); } } } BulletGlobals.GjkEpaPenetrationDepthSolverPool.Free( gjkEpaPenetrationSolver ); BulletGlobals.VoronoiSimplexSolverPool.Free( simplexSolver ); BulletGlobals.ContinuousConvexCollisionPool.Free( convexCaster1 ); } else { if( collisionShape.isConcave() ) { if( collisionShape.getShapeType() == BroadphaseNativeTypes.TRIANGLE_MESH_SHAPE_PROXYTYPE ) { //CProfileSample sample = new CProfileSample("convexSweepbtBvhTriangleMesh"); btConcaveShape triangleMesh = (btConcaveShape)collisionShape; btTransform worldTocollisionObject; colObjTransform.inverse( out worldTocollisionObject ); btVector3 convexFromLocal; worldTocollisionObject.Apply( ref convexFromTrans.m_origin, out convexFromLocal ); btVector3 convexToLocal; worldTocollisionObject.Apply( ref convexToTrans.m_origin, out convexToLocal ); // rotation of box in local mesh space = MeshRotation^-1 ConvexToRotation btTransform rotationXform; worldTocollisionObject.m_basis.Apply( ref convexToTrans.m_basis, out rotationXform.m_basis ); rotationXform.m_origin = btVector3.Zero; //ConvexCast::CastResult BridgeTriangleConvexcastCallback tccb = BulletGlobals.BridgeTriangleConvexcastCallbackPool.Get(); tccb.Initialize( castShape, ref convexFromTrans, ref convexToTrans , resultCallback, collisionObject , triangleMesh, ref colObjTransform ); tccb.m_hitFraction = resultCallback.m_closestHitFraction; tccb.m_allowedPenetration = allowedPenetration; btVector3 boxMinLocal, boxMaxLocal; castShape.getAabb( ref rotationXform, out boxMinLocal, out boxMaxLocal ); triangleMesh.performConvexcast( tccb, ref convexFromLocal, ref convexToLocal, ref boxMinLocal, ref boxMaxLocal ); BulletGlobals.BridgeTriangleConvexcastCallbackPool.Free( tccb ); } else { if( collisionShape.getShapeType() == BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE ) { btConvexCast.CastResult castResult = BulletGlobals.CastResultPool.Get(); castResult.m_allowedPenetration = allowedPenetration; castResult.m_fraction = resultCallback.m_closestHitFraction; btStaticPlaneShape planeShape = (btStaticPlaneShape)collisionShape; btContinuousConvexCollision convexCaster1 = BulletGlobals.ContinuousConvexCollisionPool.Get(); convexCaster1.Initialize( castShape, planeShape ); btConvexCast castPtr = convexCaster1; if( castPtr.calcTimeOfImpact(ref convexFromTrans, ref convexToTrans, ref colObjTransform, ref colObjTransform, castResult ) ) { //add hit if( castResult.m_normal.length2() > (double)( 0.0001 ) ) { if( castResult.m_fraction < resultCallback.m_closestHitFraction ) { castResult.m_normal.normalize(); LocalConvexResult localConvexResult = new LocalConvexResult ( collisionObject, null, ref castResult.m_normal, ref castResult.m_hitPoint, castResult.m_fraction ); bool normalInWorldSpace = true; resultCallback.addSingleResult( ref localConvexResult, normalInWorldSpace ); } } } } else { //CProfileSample sample = new CProfileSample("convexSweepConcave"); btConcaveShape concaveShape = (btConcaveShape)collisionShape; btTransform worldTocollisionObject; colObjTransform.inverse( out worldTocollisionObject ); btVector3 convexFromLocal; worldTocollisionObject.Apply( ref convexFromTrans.m_origin, out convexFromLocal ); btVector3 convexToLocal; worldTocollisionObject.Apply( ref convexToTrans.m_origin, out convexToLocal ); // rotation of box in local mesh space = MeshRotation^-1 ConvexToRotation btTransform rotationXform; worldTocollisionObject.m_basis.Apply( ref convexToTrans.m_basis, out rotationXform.m_basis ); rotationXform.m_origin = btVector3.Zero; BridgeTriangleConvexcastCallback tccb = BulletGlobals.BridgeTriangleConvexcastCallbackPool.Get(); tccb.Initialize( castShape, ref convexFromTrans, ref convexToTrans, resultCallback, collisionObject , concaveShape, ref colObjTransform ); tccb.m_hitFraction = resultCallback.m_closestHitFraction; tccb.m_allowedPenetration = allowedPenetration; btVector3 boxMinLocal, boxMaxLocal; castShape.getAabb( ref rotationXform, out boxMinLocal, out boxMaxLocal ); btVector3 rayAabbMinLocal = convexFromLocal; rayAabbMinLocal.setMin( ref convexToLocal ); btVector3 rayAabbMaxLocal = convexFromLocal; rayAabbMaxLocal.setMax( ref convexToLocal ); rayAabbMinLocal += boxMinLocal; rayAabbMaxLocal += boxMaxLocal; concaveShape.processAllTriangles( tccb, ref rayAabbMinLocal, ref rayAabbMaxLocal ); BulletGlobals.BridgeTriangleConvexcastCallbackPool.Free( tccb ); } } } else { ///@todo : use AABB tree or other BVH acceleration structure! if( collisionShape.isCompound() ) { CProfileSample sample = new CProfileSample( "convexSweepCompound" ); btCompoundShape compoundShape = (btCompoundShape)( collisionShape ); int i = 0; for( i = 0; i < compoundShape.getNumChildShapes(); i++ ) { //btTransform childTrans = compoundShape.getChildTransform( i ); btCollisionShape childCollisionShape = compoundShape.getChildShape( i ); btTransform childWorldTrans; colObjTransform.Apply( ref compoundShape.m_children.InternalArray[i].m_transform , out childWorldTrans ); LocalInfoAdder my_cb = new LocalInfoAdder( i, resultCallback ); //btCollisionObjectWrapper tmpObj = BulletGlobals.CollisionObjectWrapperPool.Get(); //tmpObj.Initialize( colObjWrap, childCollisionShape, colObjWrap.m_collisionObject, ref childWorldTrans, -1, i ); objectQuerySingleInternal( castShape, ref convexFromTrans, ref convexToTrans , childCollisionShape , collisionObject , ref childWorldTrans , my_cb, allowedPenetration ); //BulletGlobals.CollisionObjectWrapperPool.Free( tmpObj ); } } } } }
public void setTimeStepAndCounters( double collisionMarginTriangle , btDispatcherInfo dispatchInfo, btCollisionObjectWrapper convexBodyWrap , ref btTransform convexTransform , btCollisionObjectWrapper triBodyWrap , ref btTransform triBodyTransform , btManifoldResult resultOut ) { m_convexBodyWrap = convexBodyWrap; m_triBodyWrap = triBodyWrap; m_dispatchInfoPtr = dispatchInfo; m_collisionMarginTriangle = collisionMarginTriangle; m_resultOut = resultOut; //recalc aabbs btTransform convexInTriangleSpace; btTransform invTrans; triBodyTransform.inverse( out invTrans ); invTrans.Apply( ref convexTransform, out convexInTriangleSpace ); btCollisionShape convexShape = m_convexBodyWrap.getCollisionShape(); //CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody.m_collisionShape); convexShape.getAabb( ref convexInTriangleSpace, out m_aabbMin, out m_aabbMax ); double extraMargin = collisionMarginTriangle; btVector3 extra = new btVector3( extraMargin ); m_aabbMax += extra; m_aabbMin -= extra; }