public override void localGetSupportingVertex( ref btVector3 vec, out btVector3 supVertex ) { localGetSupportingVertexWithoutMargin( ref vec, out supVertex ); if( getMargin() != btScalar.BT_ZERO ) { btVector3 vecnorm = vec; if( vecnorm.length2() < ( btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON ) ) { vecnorm.setValue( (double)( -1.0 ), (double)( -1.0 ), (double)( -1.0 ) ); } vecnorm.normalize(); supVertex.AddScale( ref vecnorm, getMargin(), out supVertex ); } }
public static void btTransformAabb( ref btVector3 halfExtents, double margin, btITransform t , out btVector3 aabbMinOut, out btVector3 aabbMaxOut ) { btVector3 halfExtentsWithMargin; halfExtents.AddScale( ref btVector3.One, margin, out halfExtentsWithMargin ); btMatrix3x3 abs_b; t.getBasis().absolute( out abs_b ); btVector3 extent; halfExtentsWithMargin.dot3( ref abs_b.m_el0, ref abs_b.m_el1, ref abs_b.m_el2, out extent ); t.m_origin.Sub( ref extent, out aabbMinOut ); t.m_origin.Add( ref extent, out aabbMaxOut ); }
public static bool btRayAabb( ref btVector3 rayFrom, ref btVector3 rayTo, ref btVector3 aabbMin, ref btVector3 aabbMax, double param, ref btVector3 normal ) { btVector3 aabbHalfExtent; aabbMax.SubScale( ref aabbMin , (double)( 0.5 ), out aabbHalfExtent ); btVector3 aabbCenter; aabbMax.AddScale( ref aabbMin, (double)( 0.5 ), out aabbCenter ); btVector3 source; rayFrom.Sub( ref aabbCenter, out source ); btVector3 target; rayTo.Sub( ref aabbCenter, out target ); int sourceOutcode = btOutcode( ref source, ref aabbHalfExtent ); int targetOutcode = btOutcode( ref target, ref aabbHalfExtent ); if( ( sourceOutcode & targetOutcode ) == 0x0 ) { double lambda_enter = btScalar.BT_ZERO; double lambda_exit = param; btVector3 r; target.Sub( ref source, out r ); int i; double normSign = 1; btVector3 hitNormal = btVector3.Zero; int bit = 1; for( int j = 0; j < 2; j++ ) { for( i = 0; i != 3; ++i ) { if( ( sourceOutcode & bit ) != 0 ) { double lambda = ( -source[i] - aabbHalfExtent[i] * normSign ) / r[i]; if( lambda_enter <= lambda ) { lambda_enter = lambda; hitNormal.setValue( 0, 0, 0 ); hitNormal[i] = normSign; } } else if( ( targetOutcode & bit ) != 0 ) { double lambda = ( -source[i] - aabbHalfExtent[i] * normSign ) / r[i]; btScalar.btSetMin( ref lambda_exit, lambda ); } bit <<= 1; } normSign = (double)( -1.0); } if( lambda_enter <= lambda_exit ) { param = lambda_enter; normal = hitNormal; return true; } } return false; }
public virtual void drawSpherePatch( ref btVector3 center, ref btVector3 up, ref btVector3 axis, double radius, double minTh, double maxTh, double minPs, double maxPs, ref btVector3 color , double stepDegrees = btScalar.BT_TEN , bool drawCenter = true ) { btVector3[] vA = new btVector3[74]; btVector3[] vB = new btVector3[74]; btVector3[] vT; //btVector3* pvA = vA, *pvB = vB, *pT; btVector3 npole; center.AddScale( ref up, radius, out npole ); btVector3 spole; center.AddScale( ref up, -radius, out spole ); btVector3 arcStart = btVector3.Zero; double step = stepDegrees * btScalar.SIMD_RADS_PER_DEG; btVector3 kv = up; btVector3 iv = axis; btVector3 jv; kv.cross( ref iv, out jv ); bool drawN = false; bool drawS = false; if( minTh <= -btScalar.SIMD_HALF_PI ) { minTh = -btScalar.SIMD_HALF_PI + step; drawN = true; } if( maxTh >= btScalar.SIMD_HALF_PI ) { maxTh = btScalar.SIMD_HALF_PI - step; drawS = true; } if( minTh > maxTh ) { minTh = -btScalar.SIMD_HALF_PI + step; maxTh = btScalar.SIMD_HALF_PI - step; drawN = drawS = true; } int n_hor = (int)( ( maxTh - minTh ) / step ) + 1; if( n_hor < 2 ) n_hor = 2; double step_h = ( maxTh - minTh ) / (double)( n_hor - 1 ); bool isClosed = false; if( minPs > maxPs ) { minPs = -btScalar.SIMD_PI + step; maxPs = btScalar.SIMD_PI; isClosed = true; } else if( ( maxPs - minPs ) >= btScalar.SIMD_2_PI ) { isClosed = true; } else { isClosed = false; } int n_vert = (int)( ( maxPs - minPs ) / step ) + 1; if( n_vert < 2 ) n_vert = 2; double step_v = ( maxPs - minPs ) / (double)( n_vert - 1 ); for( int i = 0; i < n_hor; i++ ) { double th = minTh + (double)( i ) * step_h; double sth = radius * btScalar.btSin( th ); double cth = radius * btScalar.btCos( th ); for( int j = 0; j < n_vert; j++ ) { double psi = minPs + (double)( j ) * step_v; double sps = btScalar.btSin( psi ); double cps = btScalar.btCos( psi ); center.AddScale( ref iv, cth * cps, out vB[j] ); vB[j].AddScale( ref jv, cth * sps, out vB[j] ); vB[j].AddScale( ref kv, sth, out vB[j] ); //pvB[j] = center + cth * cps * iv + cth * sps * jv + sth * kv; if( i != 0 ) { drawLine( ref vA[j], ref vB[j], ref color ); } else if( drawS ) { drawLine( ref spole, ref vB[j], ref color ); } if( j != 0 ) { drawLine( ref vB[j - 1], ref vB[j], ref color ); } else { arcStart = vB[j]; } if( ( i == ( n_hor - 1 ) ) && drawN ) { drawLine( ref npole, ref vB[j], ref color ); } if( drawCenter ) { if( isClosed ) { if( j == ( n_vert - 1 ) ) { drawLine( ref arcStart, ref vB[j], ref color ); } } else { if( ( ( i== 0 ) || ( i == ( n_hor - 1 ) ) ) && ( ( j == 0 ) || ( j == ( n_vert - 1 ) ) ) ) { drawLine( ref center, ref vB[j], ref color ); } } } } vT = vA; vA = vB; vB = vT; //pT = pvA; pvA = pvB; pvB = pT; } }
public virtual void drawArc( ref btVector3 center, ref btVector3 normal, ref btVector3 axis , double radiusA, double radiusB, double minAngle, double maxAngle, ref btVector3 color, bool drawSect, double stepDegrees = btScalar.BT_TEN ) { btVector3 vx = axis; btVector3 vy; normal.cross( ref axis, out vy ); double step = stepDegrees * btScalar.SIMD_RADS_PER_DEG; int nSteps = (int)btScalar.btFabs( ( maxAngle - minAngle ) / step ); if( nSteps == 0 ) nSteps = 1; btVector3 prev; center.AddScale( ref vx, radiusA * btScalar.btCos( minAngle ), out prev ); prev.AddScale( ref vy, radiusB * btScalar.btSin( minAngle ), out prev ); if( drawSect ) { drawLine( ref center, ref prev, ref color ); } for( int i = 1; i <= nSteps; i++ ) { double angle = minAngle + ( maxAngle - minAngle ) * i / (double)nSteps; btVector3 next; center.AddScale( ref vx, radiusA * btScalar.btCos( angle ), out next ); next.AddScale( ref vy, radiusB * btScalar.btSin( angle ), out next ); drawLine( ref prev, ref next, ref color ); prev = next; } if( drawSect ) { drawLine( ref center, ref prev, ref color ); } }
public override void localGetSupportingVertex( ref btVector3 vec, out btVector3 supVertex ) { localGetSupportingVertexWithoutMargin( ref vec, out supVertex ); btVector3 vecnorm = vec; if( vecnorm.length2() < ( btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON ) ) { vecnorm = btVector3.NegOne; } vecnorm.normalize(); supVertex.AddScale( ref vecnorm, getMargin(), out supVertex ); //supVertex += getMargin() * vecnorm; }
static double projectorigin( ref btVector3 a, ref btVector3 b, double[] w, out uint m ) { btVector3 d; b.Sub( ref a, out d ); double l = d.length2(); if( l > GJK_SIMPLEX2_EPS ) { double t = ( l > 0 ? -btVector3.btDot( ref a, ref d ) / l : 0 ); if( t >= 1 ) { w[0] = 0; w[1] = 1; m = 2; return ( b.length2() ); } else if( t <= 0 ) { w[0] = 1; w[1] = 0; m = 1; return ( a.length2() ); } else { w[0] = 1 - ( w[1] = t ); m = 3; btVector3 result; a.AddScale( ref d, t, out result ); return ( result.length2() ); } } m = 10; return ( -1 ); }
public override void addContactPoint( ref btVector3 normalOnBInWorld, ref btVector3 pointInWorld, double orgDepth ) { btVector3 endPt, startPt; double newDepth; //btVector3 newNormal; if( m_perturbA ) { btVector3 endPtOrg; pointInWorld.AddScale( ref normalOnBInWorld, orgDepth, out endPtOrg ); btTransform inv; m_transformA.inverse( out inv ); btTransform tmp; m_unPerturbedTransform.Apply( ref inv, out tmp ); /*endPt =*/ tmp.Apply( ref endPtOrg, out endPt ); btVector3 tmp2; endPt.Sub( ref pointInWorld, out tmp2 ); newDepth = tmp2.dot( ref normalOnBInWorld ); endPt.AddScale( ref normalOnBInWorld, newDepth, out startPt ); } else { pointInWorld.AddScale( ref normalOnBInWorld, orgDepth, out endPt ); btTransform inv; m_transformB.inverse( out inv ); btTransform tmp; m_unPerturbedTransform.Apply( ref inv, out tmp ); tmp.Apply( ref pointInWorld, out startPt ); //startPt = ( m_unPerturbedTransform * m_transformB.inverse() )( pointInWorld ); btVector3 tmp2; endPt.Sub( ref startPt, out tmp2 ); newDepth = tmp2.dot( ref normalOnBInWorld ); } //#define DEBUG_CONTACTS 1 #if DEBUG_CONTACTS m_debugDrawer.drawLine(ref startPt,ref endPt,btVector3(1,0,0)); m_debugDrawer.drawSphere(ref startPt,0.05,btVector3(0,1,0)); m_debugDrawer.drawSphere(ref endPt,0.05,btVector3(0,0,1)); #endif //DEBUG_CONTACTS m_originalManifoldResult.addContactPoint( ref normalOnBInWorld, ref startPt, newDepth ); }
internal void calculateTransforms( ref btTransform transA, ref btTransform transB ) { if( m_useLinearReferenceFrameA /*|| ( !m_useSolveConstraintObsolete )*/ ) { transA.Apply( ref m_frameInA, out m_calculatedTransformA ); transB.Apply( ref m_frameInB, out m_calculatedTransformB ); } else { transA.Apply( ref m_frameInA, out m_calculatedTransformB ); transB.Apply( ref m_frameInB, out m_calculatedTransformA ); } m_realPivotAInW = m_calculatedTransformA.m_origin; m_realPivotBInW = m_calculatedTransformB.m_origin; m_sliderAxis = m_calculatedTransformA.m_basis.getColumn( 0 ); // along X /* if( m_useLinearReferenceFrameA || m_useSolveConstraintObsolete ) { m_delta = m_realPivotBInW - m_realPivotAInW; } else */ { m_delta = m_realPivotAInW - m_realPivotBInW; } m_realPivotAInW.AddScale( ref m_sliderAxis, m_sliderAxis.dot( ref m_delta ), out m_projPivotInW ); //m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot( ref m_delta ) * m_sliderAxis; btVector3 normalWorld; int i; //linear part for( i = 0; i < 3; i++ ) { normalWorld = m_calculatedTransformA.m_basis.getColumn( i ); m_depth[i] = m_delta.dot( normalWorld ); } }
public override void addContactPoint( ref btVector3 normalOnBInWorld, ref btVector3 pointInWorld, double depth ) { bool isSwapped = m_manifoldPtr.m_body0 != m_body0Wrap.m_collisionObject; btVector3 pointA; pointInWorld.AddScale( ref normalOnBInWorld, depth, out pointA ); btVector3 localA; btVector3 localB; if( isSwapped ) { m_body1Wrap.m_collisionObject.m_worldTransform.invXform( ref pointA, out localA ); m_body0Wrap.m_collisionObject.m_worldTransform.invXform( ref pointInWorld, out localB ); } else { m_body0Wrap.m_collisionObject.m_worldTransform.invXform( ref pointA, out localA ); m_body1Wrap.m_collisionObject.m_worldTransform.invXform( ref pointInWorld, out localB ); } btManifoldPoint newPt = BulletGlobals.ManifoldPointPool.Get(); newPt.Initialize( ref localA, ref localB, ref normalOnBInWorld, depth ); btScalar.Dbg( "2 add contact point " + pointA.ToString() + " and " + pointInWorld.ToString() ); newPt.m_positionWorldOnA = pointA; newPt.m_positionWorldOnB = pointInWorld; //BP mod, store contact triangles. if( isSwapped ) { newPt.m_partId0 = m_partId1; newPt.m_partId1 = m_partId0; newPt.m_index0 = m_index1; newPt.m_index1 = m_index0; } else { newPt.m_partId0 = m_partId0; newPt.m_partId1 = m_partId1; newPt.m_index0 = m_index0; newPt.m_index1 = m_index1; } //experimental feature info, for per-triangle material etc. btCollisionObjectWrapper obj0Wrap = isSwapped ? m_body1Wrap : m_body0Wrap; btCollisionObjectWrapper obj1Wrap = isSwapped ? m_body0Wrap : m_body1Wrap; m_resultCallback.addSingleResult( newPt, obj0Wrap, newPt.m_partId0, newPt.m_index0, obj1Wrap, newPt.m_partId1, newPt.m_index1 ); }
// See also geometrictools.com // Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv double SegmentSqrDistance( ref btVector3 from, ref btVector3 to, ref btVector3 p, out btVector3 nearest ) { btVector3 diff = p - from; btVector3 v = to - from; double t = v.dot( diff ); if( t > 0 ) { double dotVV = v.dot( v ); if( t < dotVV ) { t /= dotVV; diff.AddScale( ref v, -t, out diff ); //diff -= t * v; } else { t = 1; diff.Sub( ref v, out diff ); //diff -= v; } } else t = 0; from.AddScale( ref v, t, out nearest ); //nearest = from + t * v; return diff.dot( ref diff ); }
public virtual void addContactPoint( ref btVector3 normalOnBInWorld, ref btVector3 pointInWorld, double depth ) { Debug.Assert( m_manifoldPtr != null ); //order in manifold needs to match if( depth > m_manifoldPtr.getContactBreakingThreshold() ) // if (depth > m_manifoldPtr.getContactProcessingThreshold()) return; bool isSwapped = m_manifoldPtr.m_body0 != m_body0Wrap.m_collisionObject; btVector3 pointA;// = pointInWorld + normalOnBInWorld * depth; pointInWorld.AddScale( ref normalOnBInWorld, depth, out pointA ); btVector3 localA; btVector3 localB; if( isSwapped ) { m_body1Wrap.m_collisionObject.m_worldTransform.invXform( ref pointA, out localA ); m_body0Wrap.m_collisionObject.m_worldTransform.invXform( ref pointInWorld, out localB ); } else { m_body0Wrap.m_collisionObject.m_worldTransform.invXform( ref pointA, out localA ); m_body1Wrap.m_collisionObject.m_worldTransform.invXform( ref pointInWorld, out localB ); } btManifoldPoint newPt = BulletGlobals.ManifoldPointPool.Get(); newPt.Initialize( ref localA, ref localB, ref normalOnBInWorld, depth ); btScalar.Dbg( "add contact point " + pointA + " and " + pointInWorld ); newPt.m_positionWorldOnA = pointA; newPt.m_positionWorldOnB = pointInWorld; int insertIndex = m_manifoldPtr.getCacheEntry( ref newPt ); newPt.m_combinedFriction = calculateCombinedFriction( m_body0Wrap.m_collisionObject, m_body1Wrap.m_collisionObject ); newPt.m_combinedRestitution = calculateCombinedRestitution( m_body0Wrap.m_collisionObject, m_body1Wrap.m_collisionObject ); newPt.m_combinedRollingFriction = calculateCombinedRollingFriction( m_body0Wrap.m_collisionObject, m_body1Wrap.m_collisionObject ); btVector3.btPlaneSpace1( ref newPt.m_normalWorldOnB, out newPt.m_lateralFrictionDir1, out newPt.m_lateralFrictionDir2 ); //BP mod, store contact triangles. if( isSwapped ) { newPt.m_partId0 = m_partId1; newPt.m_partId1 = m_partId0; newPt.m_index0 = m_index1; newPt.m_index1 = m_index0; } else { newPt.m_partId0 = m_partId0; newPt.m_partId1 = m_partId1; newPt.m_index0 = m_index0; newPt.m_index1 = m_index1; } //Console.WriteLine("depth=%f\n",depth); ///@todo, check this for any side effects if( insertIndex >= 0 ) { //btManifoldPoint oldPoint = m_manifoldPtr.getContactPoint(insertIndex); m_manifoldPtr.replaceContactPoint( newPt, insertIndex ); } else { insertIndex = m_manifoldPtr.addManifoldPoint( newPt ); } //User can override friction and/or restitution if( gContactAddedCallback != null && //and if either of the two bodies requires custom material ( ( m_body0Wrap.m_collisionObject.getCollisionFlags() & btCollisionObject.CollisionFlags.CF_CUSTOM_MATERIAL_CALLBACK ) != 0 || ( m_body1Wrap.m_collisionObject.getCollisionFlags() & btCollisionObject.CollisionFlags.CF_CUSTOM_MATERIAL_CALLBACK ) != 0 ) ) { //experimental feature info, for per-triangle material etc. btCollisionObjectWrapper obj0Wrap = isSwapped ? m_body1Wrap : m_body0Wrap; btCollisionObjectWrapper obj1Wrap = isSwapped ? m_body0Wrap : m_body1Wrap; gContactAddedCallback( m_manifoldPtr.getContactPoint( insertIndex ), obj0Wrap, newPt.m_partId0, newPt.m_index0, obj1Wrap, newPt.m_partId1, newPt.m_index1 ); } }