public static void AabbExpand( ref btVector3 aabbMin, ref btVector3 aabbMax, ref btVector3 expansionMin, ref btVector3 expansionMax ) { aabbMin.Add( ref expansionMin, out aabbMin ); aabbMin.Add( ref expansionMax, out aabbMin ); }
public override void drawContactPoint( ref btVector3 PointOnB, ref btVector3 normalOnB, double distance, int lifeTime, ref btVector3 color ) { btVector3 tmpD; PointOnB.Add( ref normalOnB, out tmpD ); drawLine( ref PointOnB, ref tmpD, ref color, ref color ); }
public override void setMargin( double collisionMargin ) { //correct the m_implicitShapeDimensions for the margin btVector3 oldMargin = new btVector3( getMargin(), getMargin(), getMargin() ); btVector3 implicitShapeDimensionsWithMargin; oldMargin.Add( ref m_implicitShapeDimensions, out implicitShapeDimensionsWithMargin ); base.setMargin( collisionMargin ); btVector3 newMargin = new btVector3( getMargin(), getMargin(), getMargin() ); implicitShapeDimensionsWithMargin.Sub( ref newMargin, out m_implicitShapeDimensions ); }
public override void getAabb( ref btTransform t, out btVector3 aabbMin, out btVector3 aabbMax ) { btVector3 halfExtents = new btVector3( getRadius(), getRadius(), getRadius() ); btVector3 tmp = halfExtents; halfExtents[m_upAxis] = getRadius() + getHalfHeight(); halfExtents.Add( ref tmp, out halfExtents ); btMatrix3x3 abs_b; t.m_basis.absolute( out abs_b ); btVector3 extent; halfExtents.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 aabbMin ); t.m_origin.Add( ref extent, out aabbMax ); }
public override void localGetSupportingVertex( ref btVector3 vec, out btVector3 result ) { coneLocalSupport( ref vec, out result ); if( getMargin() != btScalar.BT_ZERO ) { btVector3 vecnorm = vec; if( vecnorm.length2() < ( btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON ) ) { vecnorm = btVector3.NegOne; } vecnorm.normalize(); vecnorm.Mult( getMargin(), out vecnorm ); result.Add( ref vecnorm, out result ); } }
internal eStatus._ Evaluate( tShape shapearg, ref btVector3 guess ) { uint iterations = 0; double sqdist = 0; double alpha = 0; btVector3[] lastw = new btVector3[4]; uint clastw = 0; /* Initialize solver */ m_free[0] = new sSV(); m_free[1] = new sSV(); m_free[2] = new sSV(); m_free[3] = new sSV(); m_nfree = 4; m_current = 0; m_status = eStatus._.Valid; m_shape = shapearg; m_distance = 0; /* Initialize simplex */ m_simplices0.rank = 0; m_ray = guess; double sqrl = m_ray.length2(); btVector3 tmp; if( sqrl > 0 ) m_ray.Invert( out tmp ); else tmp = btVector3.xAxis; appendvertice( m_simplices0, ref tmp ); m_simplices0.p[0] = 1; m_ray = m_simplices0.c[0].w; sqdist = sqrl; lastw[0] = lastw[1] = lastw[2] = lastw[3] = m_ray; /* Loop */ do { uint next = 1 - m_current; sSimplex cs = m_current==0?m_simplices0:m_simplices1; sSimplex ns = next==0?m_simplices0:m_simplices1; /* Check zero */ double rl = m_ray.length(); if( rl < GJK_MIN_DISTANCE ) {/* Touching or inside */ m_status = eStatus._.Inside; break; } /* Append new vertice in -'v' direction */ m_ray.Invert( out tmp ); appendvertice( cs, ref tmp ); btVector3 w = cs.c[cs.rank - 1].w; bool found = false; for( uint i = 0; i < 4; ++i ) { w.Sub( ref lastw[i], out tmp ); if( tmp.length2() < GJK_DUPLICATED_EPS ) { found = true; break; } } if( found ) {/* Return old simplex */ removevertice( cs ); break; } else {/* Update lastw */ lastw[clastw = ( clastw + 1 ) & 3] = w; } /* Check for termination */ double omega = btVector3.btDot( ref m_ray, ref w ) / rl; alpha = btScalar.btMax( omega, alpha ); if( ( ( rl - alpha ) - ( GJK_ACCURARY * rl ) ) <= 0 ) {/* Return old simplex */ removevertice( cs ); break; } /* Reduce simplex */ double[] weights = new double[4]; uint mask = 0; switch( cs.rank ) { case 2: sqdist = projectorigin( ref cs.c[0].w, ref cs.c[1].w, weights, out mask ); break; case 3: sqdist = projectorigin( ref cs.c[0].w, ref cs.c[1].w, ref cs.c[2].w, weights, out mask ); break; case 4: sqdist = projectorigin( ref cs.c[0].w, ref cs.c[1].w, ref cs.c[2].w, ref cs.c[3].w, weights, out mask ); break; } if( sqdist >= 0 ) {/* Valid */ ns.rank = 0; m_ray = btVector3.Zero; m_current = next; for( int i = 0, ni = (int)cs.rank; i < ni; ++i ) { if( ( mask & ( (uint)1 << i ) ) != 0 ) { ns.c[ns.rank] = cs.c[i]; ns.p[ns.rank++] = weights[i]; btVector3 tmp2; cs.c[i].w.Mult( weights[i], out tmp2 ); m_ray.Add( ref tmp2, out m_ray ); } else { m_free[m_nfree++] = cs.c[i]; } } if( mask == 15 ) m_status = eStatus._.Inside; } else {/* Return old simplex */ removevertice( cs ); break; } m_status = ( ( ++iterations ) < GJK_MAX_ITERATIONS ) ? m_status : eStatus._.Failed; } while( m_status == eStatus._.Valid ); m_simplex = m_current==0?m_simplices0:m_simplices1; switch( m_status ) { case eStatus._.Valid: m_distance = m_ray.length(); break; case eStatus._.Inside: m_distance = 0; break; default: break; } return ( m_status ); }
public void getAabbNonVirtual( ref btTransform t, out btVector3 aabbMin, out btVector3 aabbMax ) { switch( m_shapeType ) { case BroadphaseNativeTypes.SPHERE_SHAPE_PROXYTYPE: { btSphereShape sphereShape = (btSphereShape)this; double radius = sphereShape.getImplicitShapeDimensions().x;// * convexShape.getLocalScaling().x; double margin = radius + sphereShape.getMarginNonVirtual(); btVector3 extent = new btVector3( margin, margin, margin ); t.m_origin.Sub( ref extent, out aabbMin ); t.m_origin.Add( ref extent, out aabbMax ); } break; case BroadphaseNativeTypes.CYLINDER_SHAPE_PROXYTYPE: /* fall through */ case BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE: { btBoxShape convexShape = (btBoxShape)this; double margin = convexShape.getMarginNonVirtual(); btVector3 halfExtents = convexShape.getImplicitShapeDimensions(); btVector3 tmp = new btVector3( margin, margin, margin ); halfExtents.Add( ref tmp, out halfExtents ); btMatrix3x3 abs_b; t.m_basis.absolute( out abs_b ); btVector3 extent; halfExtents.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 aabbMin ); t.m_origin.Add( ref extent, out aabbMax ); break; } case BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE: { btTriangleShape triangleShape = (btTriangleShape)this; double margin = triangleShape.getMarginNonVirtual(); aabbMax = aabbMin = btVector3.Zero; for( int i = 0; i < 3; i++ ) { btVector3 vec = btVector3.Zero; vec[i] = (double)( 1.0 ); btVector3 tmp; t.m_basis.ApplyInverse( ref vec, out tmp ); btVector3 sv; localGetSupportVertexWithoutMarginNonVirtual( ref tmp, out sv ); t.Apply( ref sv, out tmp ); aabbMax[i] = tmp[i] + margin; vec[i] = (double)( -1.0); t.m_basis.ApplyInverse( ref vec, out tmp ); localGetSupportVertexWithoutMarginNonVirtual( ref tmp, out sv ); t.Apply( ref sv, out tmp ); aabbMin[i] = tmp[i] - margin; } } break; case BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE: { btCapsuleShape capsuleShape = (btCapsuleShape)this; btVector3 halfExtents = new btVector3( capsuleShape.getRadius(), capsuleShape.getRadius(), capsuleShape.getRadius()); int m_upAxis = capsuleShape.getUpAxis(); halfExtents[m_upAxis] = capsuleShape.getRadius() + capsuleShape.getHalfHeight(); btVector3 tmp = new btVector3( capsuleShape.getMarginNonVirtual(), capsuleShape.getMarginNonVirtual(), capsuleShape.getMarginNonVirtual() ); halfExtents.Add( ref tmp, out halfExtents ); btMatrix3x3 abs_b; t.m_basis.absolute( out abs_b ); btVector3 extent; halfExtents.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 aabbMin ); t.m_origin.Add( ref extent, out aabbMax ); } break; case BroadphaseNativeTypes.CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE: case BroadphaseNativeTypes.CONVEX_HULL_SHAPE_PROXYTYPE: { btPolyhedralConvexAabbCachingShape convexHullShape = (btPolyhedralConvexAabbCachingShape)this; double margin = convexHullShape.getMarginNonVirtual(); convexHullShape.getNonvirtualAabb( ref t, out aabbMin, out aabbMax, margin ); } break; default: getAabb( ref t, out aabbMin, out aabbMax ); break; } // should never reach here Debug.Assert( false ); aabbMin = aabbMax = btVector3.Zero; }
public void localGetSupportVertexNonVirtual( ref btVector3 localDir, out btVector3 result ) { btVector3 localDirNorm = localDir; if( localDirNorm.length2() < ( btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON ) ) { localDirNorm = btVector3.NegOne; } localDirNorm.normalize(); localDirNorm.Mult( getMarginNonVirtual(), out localDirNorm ); localGetSupportVertexWithoutMarginNonVirtual( ref localDirNorm, out result ); result.Add( ref localDirNorm, out result ); }
public void getHalfExtentsWithMargin( out btVector3 halfExtents ) { getHalfExtentsWithoutMargin( out halfExtents ); btVector3 margin = new btVector3( getMargin(), getMargin(), getMargin() ); halfExtents.Add( ref margin, out halfExtents ); }
public void PlaneLineIntersection( ref btVector3 p0, ref btVector3 p1, out btVector3 result ) { // returns the point where the line p0-p1 intersects the plane n&d btVector3 dif; p1.Sub( ref p0, out dif ); double dn = normal.dot( ref dif ); double t = -( dist + normal.dot( ref p0 ) ) / dn; dif.Mult( t, out dif ); p0.Add( ref dif, out result ); }
public static double DistanceBetweenLines( ref btVector3 ustart, ref btVector3 udir , ref btVector3 vstart, ref btVector3 vdir , out btVector3 upoint, out btVector3 vpoint ) { btVector3 cp; udir.cross( ref vdir, out cp ); cp.normalize(); double distu = -cp.dot( ref ustart ); double distv = -cp.dot( ref vstart ); double dist = btScalar.btFabs( distu - distv ); btPlane plane; btVector3 tmp; //if( upoint ) { vdir.cross( ref cp, out plane.normal ); plane.normal.normalize(); plane.dist = -plane.normal.dot( ref vstart ); ustart.Add( ref udir, out tmp ); plane.PlaneLineIntersection( ref ustart, ref tmp, out upoint ); } //if( vpoint ) { udir.cross( ref cp, out plane.normal ); plane.normal.normalize(); plane.dist = -plane.normal.dot( ref ustart ); ustart.Add( ref vdir, out tmp ); plane.PlaneLineIntersection( ref vstart, ref tmp, out vpoint ); } return dist; }
public bool closestPtPointTriangle( ref btVector3 p, ref btVector3 a, ref btVector3 b, ref btVector3 c, ref btSubSimplexClosestResult result ) { result.m_usedVertices = 0;//.reset(); // Check if P in vertex region outside A btVector3 ab; b.Sub( ref a, out ab ); btVector3 ac; c.Sub( ref a, out ac ); btVector3 ap; p.Sub( ref a, out ap ); double d1 = ab.dot( ref ap ); double d2 = ac.dot( ref ap ); if( d1 <= (double)( 0.0 ) && d2 <= (double)( 0.0 ) ) { result.m_closestPointOnSimplex = a; result.m_usedVertices |= btUsageBitfield.usedVertexA; result.setBarycentricCoordinates( 1, 0, 0 ); return true;// a; // barycentric coordinates (1,0,0) } // Check if P in vertex region outside B btVector3 bp; p.Sub( ref b, out bp ); double d3 = ab.dot( ref bp ); double d4 = ac.dot( ref bp ); if( d3 >= (double)( 0.0 ) && d4 <= d3 ) { result.m_closestPointOnSimplex = b; result.m_usedVertices |= btUsageBitfield.usedVertexB; result.setBarycentricCoordinates( 0, 1, 0 ); return true; // b; // barycentric coordinates (0,1,0) } // Check if P in edge region of AB, if so return projection of P onto AB double vc = d1 * d4 - d3 * d2; btVector3 tmp; if( vc <= (double)( 0.0 ) && d1 >= (double)( 0.0 ) && d3 <= (double)( 0.0 ) ) { double v = d1 / ( d1 - d3 ); ab.Mult( v, out tmp ); a.Add( ref tmp, out result.m_closestPointOnSimplex ); result.m_usedVertices |= btUsageBitfield.usedVertexA; result.m_usedVertices |= btUsageBitfield.usedVertexB; result.setBarycentricCoordinates( 1 - v, v, 0 ); return true; //return a + v * ab; // barycentric coordinates (1-v,v,0) } // Check if P in vertex region outside C btVector3 cp; p.Sub( ref c, out cp ); double d5 = ab.dot( ref cp ); double d6 = ac.dot( ref cp ); if( d6 >= (double)( 0.0 ) && d5 <= d6 ) { result.m_closestPointOnSimplex = c; result.m_usedVertices |= btUsageBitfield.usedVertexC; result.setBarycentricCoordinates( 0, 0, 1 ); return true;//c; // barycentric coordinates (0,0,1) } // Check if P in edge region of AC, if so return projection of P onto AC double vb = d5 * d2 - d1 * d6; if( vb <= (double)( 0.0 ) && d2 >= (double)( 0.0 ) && d6 <= (double)( 0.0 ) ) { double w = d2 / ( d2 - d6 ); ac.Mult( w, out tmp ); a.Add( ref tmp, out result.m_closestPointOnSimplex ); result.m_usedVertices |= btUsageBitfield.usedVertexA; result.m_usedVertices |= btUsageBitfield.usedVertexC; result.setBarycentricCoordinates( 1 - w, 0, w ); return true; //return a + w * ac; // barycentric coordinates (1-w,0,w) } // Check if P in edge region of BC, if so return projection of P onto BC double va = d3 * d6 - d5 * d4; if( va <= (double)( 0.0 ) && ( d4 - d3 ) >= (double)( 0.0 ) && ( d5 - d6 ) >= (double)( 0.0 ) ) { double w = ( d4 - d3 ) / ( ( d4 - d3 ) + ( d5 - d6 ) ); btVector3 tmp2; c.Sub( ref b, out tmp2 ); tmp2.Mult( w, out tmp ); b.Add( ref tmp, out result.m_closestPointOnSimplex ); //result.m_closestPointOnSimplex = b + w * ( c - b ); result.m_usedVertices |= btUsageBitfield.usedVertexB; result.m_usedVertices |= btUsageBitfield.usedVertexC; result.setBarycentricCoordinates( 0, 1 - w, w ); return true; // return b + w * (c - b); // barycentric coordinates (0,1-w,w) } // P inside face region. Compute Q through its barycentric coordinates (u,v,w) { double denom = (double)( 1.0 ) / ( va + vb + vc ); double v = vb * denom; double w = vc * denom; ab.Mult( v, out ab ); ac.Mult( w, out ac ); a.Add( ref ab, out tmp ); tmp.Add( ref ac, out result.m_closestPointOnSimplex ); //result.m_closestPointOnSimplex = a + ab * v + ac * w; result.m_usedVertices |= btUsageBitfield.usedVertexA; result.m_usedVertices |= btUsageBitfield.usedVertexB; result.m_usedVertices |= btUsageBitfield.usedVertexC; result.setBarycentricCoordinates( 1 - v - w, v, w ); } return true; // return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = (double)(1.0) - v - w }
///calculateTemporalAabb calculates the enclosing aabb for the moving object over interval [0..timeStep) ///result is conservative public void calculateTemporalAabb( ref btTransform curTrans, ref btVector3 linvel, ref btVector3 angvel, double timeStep , out btVector3 temporalAabbMin, out btVector3 temporalAabbMax ) { //start with static aabb getAabb( ref curTrans, out temporalAabbMin, out temporalAabbMax ); double temporalAabbMaxx = temporalAabbMax.x; double temporalAabbMaxy = temporalAabbMax.y; double temporalAabbMaxz = temporalAabbMax.z; double temporalAabbMinx = temporalAabbMin.x; double temporalAabbMiny = temporalAabbMin.y; double temporalAabbMinz = temporalAabbMin.z; // add linear motion btVector3 linMotion; linvel.Mult( timeStep, out linMotion ); ///@todo: simd would have a vector max/min operation, instead of per-element access if( linMotion.x > btScalar.BT_ZERO ) temporalAabbMaxx += linMotion.x; else temporalAabbMinx += linMotion.x; if( linMotion.y > btScalar.BT_ZERO ) temporalAabbMaxy += linMotion.y; else temporalAabbMiny += linMotion.y; if( linMotion.z > btScalar.BT_ZERO ) temporalAabbMaxz += linMotion.z; else temporalAabbMinz += linMotion.z; //add conservative angular motion double angularMotion = angvel.length() * getAngularMotionDisc() * timeStep; btVector3 angularMotion3d = new btVector3( angularMotion, angularMotion, angularMotion ); temporalAabbMin = new btVector3( temporalAabbMinx, temporalAabbMiny, temporalAabbMinz ); temporalAabbMax = new btVector3( temporalAabbMaxx, temporalAabbMaxy, temporalAabbMaxz ); temporalAabbMin.Sub( ref angularMotion3d, out temporalAabbMin ); temporalAabbMax.Add( ref angularMotion3d, out temporalAabbMax ); }