public void drawSphere(float radius,ref btTransform transform,ref btVector3 color) { btVector3 start = transform.Origin; btVector3 xoffs;// = btMatrix3x3.Multiply(transform.Basis, new btVector3(radius, 0, 0)); btVector3 yoffs;// = btMatrix3x3.Multiply(transform.Basis, new btVector3(0, radius, 0)); btVector3 zoffs;// = btMatrix3x3.Multiply(transform.Basis, new btVector3(0, 0, radius)); { btVector3 temp; temp = new btVector3(radius, 0, 0); btMatrix3x3.Multiply(ref transform.Basis, ref temp, out xoffs); temp = new btVector3(0, radius, 0); btMatrix3x3.Multiply(ref transform.Basis, ref temp, out yoffs); temp = new btVector3(0, 0, radius); btMatrix3x3.Multiply(ref transform.Basis, ref temp, out zoffs); } // XY /*drawLine(start - xoffs, start + yoffs, color); drawLine(start + yoffs, start + xoffs, color); drawLine(start + xoffs, start - yoffs, color); drawLine(start - yoffs, start - xoffs, color); // XZ drawLine(start - xoffs, start + zoffs, color); drawLine(start + zoffs, start + xoffs, color); drawLine(start + xoffs, start - zoffs, color); drawLine(start - zoffs, start - xoffs, color); // YZ drawLine(start - yoffs, start + zoffs, color); drawLine(start + zoffs, start + yoffs, color); drawLine(start + yoffs, start - zoffs, color); drawLine(start - zoffs, start - yoffs, color);*/ btVector3 sax, say, saz, smx, smy, smz; btVector3.Add(ref start, ref xoffs, out sax); btVector3.Add(ref start, ref yoffs, out say); btVector3.Add(ref start, ref zoffs, out saz); btVector3.Subtract(ref start, ref xoffs, out smx); btVector3.Subtract(ref start, ref yoffs, out smy); btVector3.Subtract(ref start, ref zoffs, out smz); // XY drawLine(ref smx, ref say, ref color); drawLine(ref say, ref sax, ref color); drawLine(ref sax, ref smy, ref color); drawLine(ref smy, ref smx, ref color); // XZ drawLine(ref smx, ref saz, ref color); drawLine(ref saz, ref sax, ref color); drawLine(ref sax, ref smz, ref color); drawLine(ref smz, ref smx, ref color); // YZ drawLine(ref smy, ref saz, ref color); drawLine(ref saz, ref say, ref color); drawLine(ref say, ref smz, ref color); drawLine(ref smz, ref smy, ref color); }
public bool calcPenDepth(ISimplexSolver simplexSolver, ConvexShape pConvexA, ConvexShape pConvexB, btTransform transformA, btTransform transformB, ref btVector3 v, out btVector3 wWitnessOnA, out btVector3 wWitnessOnB, IDebugDraw debugDraw) { btVector3 guessVector;// = transformA.Origin - transformB.Origin; btVector3.Subtract(ref transformA.Origin,ref transformB.Origin, out guessVector); GjkEpaSolver2.sResults results = new GjkEpaSolver2.sResults(); if (GjkEpaSolver2.Penetration(pConvexA, transformA, pConvexB, transformB, guessVector, ref results)) { // debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0)); //resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth); wWitnessOnA = results.witnesses0; wWitnessOnB = results.witnesses1; v = results.normal; return true; } else { if (GjkEpaSolver2.Distance(pConvexA, transformA, pConvexB, transformB, guessVector, ref results)) { wWitnessOnA = results.witnesses0; wWitnessOnB = results.witnesses1; v = results.normal; return false; } } wWitnessOnA = new btVector3(); wWitnessOnB = new btVector3(); return false; }
public BroadphaseProxy createProxy(ref btVector3 aabbMin,ref btVector3 aabbMax, BroadphaseNativeTypes shapeType, object userPtr, short collisionFilterGroup, short collisionFilterMask, IDispatcher dispatcher, object multiSapProxy) { DbvtProxy proxy = new DbvtProxy(aabbMin, aabbMax, userPtr, collisionFilterGroup, collisionFilterMask); DbvtAabbMm aabb;// = DbvtAabbMm.FromMM(aabbMin, aabbMax); DbvtAabbMm.FromMM(ref aabbMin, ref aabbMax, out aabb); //bproxy->aabb = btDbvtVolume::FromMM(aabbMin,aabbMax); proxy.stage = m_stageCurrent; proxy.m_uniqueId = ++m_gid; proxy.leaf = m_sets[0].insert(ref aabb, proxy); listappend(ref proxy, ref m_stageRoots[m_stageCurrent]); if (!m_deferedcollide) { DbvtTreeCollider collider = new DbvtTreeCollider(this); collider.proxy = proxy; m_sets[0].collideTV(m_sets[0].m_root, ref aabb, collider); m_sets[1].collideTV(m_sets[1].m_root,ref aabb, collider); } return (proxy); }
public void Support(ref btVector3 d, uint index,out btVector3 result) { if (index != 0) Support1(ref d,out result); else Support0(ref d,out result); }
//brute force implementations public override void localGetSupportingVertex(ref btVector3 vec0, out btVector3 supVec) { supVec = btVector3.Zero; int i; float maxDot = -BulletGlobal.BT_LARGE_FLOAT; btVector3 vec = vec0; float lenSqr = vec.Length2; if (lenSqr < 0.0001f) { vec.setValue(1, 0, 0); } else { float rlen = 1f / (float)Math.Sqrt(lenSqr); vec *= rlen; } btVector3 vtx; float newDot; for (i = 0; i < NumVertices; i++) { getVertex(i, out vtx); newDot = vec.dot(vtx); if (newDot > maxDot) { maxDot = newDot; supVec = vtx; } } //return supVec; }
public static void Add(ref btVector3 v1, ref btVector3 v2, out btVector3 result) { result.X = v1.X + v2.X; result.Y = v1.Y + v2.Y; result.Z = v1.Z + v2.Z; result.W = 0; }
public void updateSingleAabb(CollisionObject colObj) { btVector3 minAabb, maxAabb; colObj.CollisionShape.getAabb(colObj.WorldTransform, out minAabb, out maxAabb); //need to increase the aabb for contact thresholds btVector3 contactThreshold = new btVector3(PersistentManifold.gContactBreakingThreshold, PersistentManifold.gContactBreakingThreshold, PersistentManifold.gContactBreakingThreshold); //minAabb -= contactThreshold; //maxAabb += contactThreshold; minAabb.Subtract(ref contactThreshold); maxAabb.Add(ref contactThreshold); //IBroadphaseInterface bp = m_broadphasePairCache; //moving objects should be moderately sized, probably something wrong if not if (colObj.isStaticObject || ((maxAabb - minAabb).Length2 < 1e12f)) { m_broadphasePairCache.setAabb(colObj.BroadphaseHandle,ref minAabb,ref maxAabb, m_dispatcher1); } else { //something went wrong, investigate //this assert is unwanted in 3D modelers (danger of loosing work) colObj.ActivationState = ActivationStateFlags.DISABLE_SIMULATION; 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"); } } }
public override void batchedUnitVectorGetSupportingVertexWithoutMargin(btVector3[] vectors, btVector3[] supportVerticesOut, int numVectors) { int i; btVector3 vtx; float newDot; for (i = 0; i < numVectors; i++) { supportVerticesOut[i].W = -BulletGlobal.BT_LARGE_FLOAT; } for (int j = 0; j < numVectors; j++) { btVector3 vec = vectors[j]; for (i = 0; i < NumVertices; i++) { getVertex(i, out vtx); newDot = vec.dot(vtx); if (newDot > supportVerticesOut[j].W) { //WARNING: don't swap next lines, the w component would get overwritten! supportVerticesOut[j] = vtx; supportVerticesOut[j].W = newDot; } } } }
public static void Multiply(ref btVector3 v, ref btMatrix3x3 m, out btVector3 result) { result.X = m.tdotx(ref v); result.Y = m.tdoty(ref v); result.Z = m.tdotz(ref v); result.W = 0; }
public static void calculateVelocity(btTransform transform0,btTransform transform1,float timeStep,out btVector3 linVel, out btVector3 angVel) { linVel = (transform1.Origin - transform0.Origin) / timeStep; btVector3 axis; float angle; calculateDiffAxisAngle(transform0,transform1,out axis,out angle); angVel = axis * angle / timeStep; }
public void reset() { m_cachedValidClosest = false; m_numVertices = 0; m_needsUpdate = true; m_lastW = new btVector3(BulletGlobal.BT_LARGE_FLOAT, BulletGlobal.BT_LARGE_FLOAT, BulletGlobal.BT_LARGE_FLOAT); m_cachedBC.reset(); }
public override void localGetSupportingVertexWithoutMargin(ref btVector3 vec,out btVector3 result) { btVector3 halfExtents = HalfExtentsWithoutMargin; result= new btVector3((vec.X >= 0 ? halfExtents.X : -halfExtents.X), (vec.Y >= 0 ? halfExtents.Y : -halfExtents.Y), (vec.Z >= 0 ? halfExtents.Z : -halfExtents.Z)); }
/// <summary> /// 線の描画 /// </summary> /// <param name="from">開始点</param> /// <param name="to">終了点</param> /// <param name="color">色</param> /// <remarks>スーパークラスから呼び出される</remarks> public override void drawLine(ref btVector3 from, ref btVector3 to, ref btVector3 color) { lines[primitiveCount * 2].pos = new Vector3(from.X, from.Y, from.Z); lines[primitiveCount * 2].color = ((int)(color.X*256)) * (2 ^ 16) + ((int)(color.Y*256)) * (2 ^ 8) + ((int)(color.Z*256)); lines[primitiveCount * 2 + 1].pos = new Vector3(to.X, to.Y, to.Z); lines[primitiveCount * 2 + 1].color = ((int)(color.X * 256)) * (2 ^ 16) + ((int)(color.Y * 256)) * (2 ^ 8) + ((int)(color.Z * 256)); ++primitiveCount; }
public static void Multiply(ref btMatrix3x3 m, ref btVector3 v, out btVector3 result) { //result = new btVector3(); result.X = m.el0.dot(v); result.Y = m.el1.dot(v); result.Z = m.el2.dot(v); result.W = 0; }
public btMatrix3x3(float xx, float xy, float xz, float yx, float yy, float yz, float zx, float zy, float zz) { el0=new btVector3(xx, xy, xz); el1 = new btVector3(yx, yy, yz); el2 = new btVector3(zx, zy, zz); }
public BroadphaseProxy(btVector3 aabbMin, btVector3 aabbMax, object userPtr, short collisionFilterGroup, short collisionFilterMask) { m_clientObject = userPtr; m_collisionFilterGroup = collisionFilterGroup; m_collisionFilterMask = collisionFilterMask; m_aabbMin = aabbMin; m_aabbMax = aabbMax; m_multiSapParentProxy = null; }
public void Support(ref btVector3 d, out btVector3 result) { //return (Support0(d) - Support1(-d)); btVector3 temp1, temp2, temp3; temp1 = -d; Support0(ref d, out temp2); Support1(ref temp1, out temp3); btVector3.Subtract(ref temp2, ref temp3, out result); }
public override void processCollision(CollisionObject col0, CollisionObject col1, DispatcherInfo dispatchInfo, ref ManifoldResult resultOut) { if (m_manifoldPtr == null) return; resultOut.PersistentManifold = m_manifoldPtr; SphereShape sphere0 = (SphereShape)col0.CollisionShape; SphereShape sphere1 = (SphereShape)col1.CollisionShape; btVector3 diff = col0.WorldTransform.Origin - col1.WorldTransform.Origin; float len = diff.Length; float radius0 = sphere0.Radius; float radius1 = sphere1.Radius; #if CLEAR_MANIFOLD m_manifoldPtr->clearManifold(); //don't do this, it disables warmstarting #endif ///iff distance positive, don't generate a new contact if (len > (radius0 + radius1)) { #if! CLEAR_MANIFOLD resultOut.refreshContactPoints(); #endif //CLEAR_MANIFOLD return; } ///distance (negative means penetration) float dist = len - (radius0 + radius1); btVector3 normalOnSurfaceB = new btVector3(1, 0, 0); if (len > BulletGlobal.SIMD_EPSILON) { //normalOnSurfaceB = diff / len; btVector3.Divide(ref diff, len, out normalOnSurfaceB); } ///point on A (worldspace) ///btVector3 pos0 = col0->getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB; ///point on B (worldspace) btVector3 pos1;// = col1.WorldTransform.Origin + radius1 * normalOnSurfaceB; { btVector3 temp; btVector3.Multiply(ref normalOnSurfaceB, radius1, out temp); btVector3.Add(ref col1.WorldTransform.Origin, ref temp, out pos1); } /// report a contact. internally this will be kept persistent, and contact reduction is done resultOut.addContactPoint(ref normalOnSurfaceB, ref pos1, dist); #if! CLEAR_MANIFOLD resultOut.refreshContactPoints(); #endif //CLEAR_MANIFOLD }
public btQuaternion(btVector3 axis, float angle) { float d = axis.Length; Debug.Assert(d != 0.0f); float s = (float)Math.Sin(angle * 0.5f) / d; X = axis.X * s; Y = axis.Y * s; Z = axis.Z * s; W = (float)Math.Cos(angle * 0.5f); }
void Support1(ref btVector3 d,out btVector3 result) { #region return (m_toshape0 * (m_shapes1).Ls(m_toshape1 * d)); btVector3 temp,temp2; btMatrix3x3.Multiply(ref m_toshape1, ref d, out temp); //return (m_toshape0 * (m_shapes1).Ls(temp)); (m_shapes1).Ls(ref temp, out temp2); btTransform.Multiply(ref m_toshape0, ref temp2, out result); #endregion }
public StaticPlaneShape(btVector3 planeNormal, float planeConstant) : base() { //m_planeNormal = planeNormal.normalized(); planeNormal.normalized(out m_planeNormal); m_planeConstant = planeConstant; m_localScaling = btVector3.Zero; // btAssert( btFuzzyZero(m_planeNormal.length() - btScalar(1.)) ); }
public void addVertex(btVector3 w, btVector3 p, btVector3 q) { m_lastW = w; m_needsUpdate = true; m_simplexVectorW[m_numVertices] = w; m_simplexPointsP[m_numVertices] = p; m_simplexPointsQ[m_numVertices] = q; m_numVertices++; }
public override void localGetSupportingVertex(ref btVector3 vec,out btVector3 result) { btVector3 halfExtents = HalfExtentsWithoutMargin; btVector3 margin = new btVector3(Margin, Margin, Margin); //halfExtents += margin; halfExtents.Add(ref margin); result= new btVector3((vec.X >= 0 ? halfExtents.X : -halfExtents.X), (vec.Y >= 0 ? halfExtents.Y : -halfExtents.Y), (vec.Z >= 0 ? halfExtents.Z : -halfExtents.Z)); }
public override void batchedUnitVectorGetSupportingVertexWithoutMargin(btVector3[] vectors, btVector3[] supportVerticesOut, int numVectors) { btVector3 halfExtents = HalfExtentsWithoutMargin; for (int i = 0; i < numVectors; i++) { btVector3 vec = vectors[i]; supportVerticesOut[i].setValue((vec.X >= 0 ? halfExtents.X : -halfExtents.X), (vec.Y >= 0 ? halfExtents.Y : -halfExtents.Y), (vec.Z >= 0 ? halfExtents.Z : -halfExtents.Z)); } }
private void Constructor() { m_ray = new btVector3(0, 0, 0); m_nfree = 0; m_status = eStatus.Failed; m_current = 0; m_distance = 0; for (int i = 0; i < m_simplices.Length; i++) m_simplices[i].Constructor(); for (int i = 0; i < m_store.Length; i++) m_store[i].Constructor(); }
public void addContactPoint(btVector3 normalOnBInWorld,btVector3 pointInWorld,float orgDepth,ref ManifoldResult originalManifoldResult) { btVector3 endPt,startPt; float newDepth; //btVector3 newNormal; if (m_perturbA) { btVector3 endPtOrg;// = pointInWorld + normalOnBInWorld*orgDepth; { btVector3 temp; btVector3.Multiply(ref normalOnBInWorld, orgDepth, out temp); btVector3.Add(ref pointInWorld, ref temp, out endPtOrg); } endPt = btVector3.Transform(endPtOrg, m_unPerturbedTransform * m_transformA.inverse()); #region newDepth = (endPt - pointInWorld).dot(normalOnBInWorld); { btVector3 temp; btVector3.Subtract(ref endPt, ref pointInWorld, out temp); newDepth = temp.dot(ref normalOnBInWorld); } #endregion #region startPt = endPt + normalOnBInWorld * newDepth; { btVector3 temp; btVector3.Multiply(ref normalOnBInWorld, newDepth, out temp); btVector3.Multiply(ref endPt, ref temp, out startPt); } #endregion } else { #region endPt = pointInWorld + normalOnBInWorld*orgDepth; { btVector3 temp; btVector3.Multiply(ref normalOnBInWorld, orgDepth, out temp); btVector3.Add(ref pointInWorld, ref temp, out endPt); } #endregion startPt = btVector3.Transform(pointInWorld, m_unPerturbedTransform * m_transformB.inverse()); #region newDepth = (endPt - startPt).dot(normalOnBInWorld); { btVector3 temp; btVector3.Subtract(ref endPt, ref startPt, out temp); newDepth = temp.dot(ref normalOnBInWorld); } #endregion } originalManifoldResult.addContactPoint(ref normalOnBInWorld,ref startPt,newDepth); }
public btMatrix3x3(btQuaternion q) { float d = q.Length2; Debug.Assert(d != 0.0f); float s = 2.0f / d; float xs = q.X * s, ys = q.Y * s, zs = q.Z * s; float wx = q.W * xs, wy = q.W * ys, wz = q.W * zs; float xx = q.X * xs, xy = q.X * ys, xz = q.X * zs; float yy = q.Y * ys, yz = q.Y * zs, zz = q.Z * zs; el0 = new btVector3(1.0f - (yy + zz), xy - wz, xz + wy); el1 = new btVector3(xy + wz, 1.0f - (xx + zz), yz - wx); el2 = new btVector3(xz - wy, yz + wx, 1.0f - (xx + yy)); }
public static bool Penetration(ConvexShape shape0, btTransform wtrs0, ConvexShape shape1, btTransform wtrs1, btVector3 guess, ref sResults results, bool usemargins) { tShape shape=new MinkowskiDiff(); Initialize(shape0,wtrs0,shape1,wtrs1,ref results,ref shape,usemargins); using (GJK gjk = GJK.CreateFromPool()) { GJK.eStatus gjk_status = gjk.Evaluate(shape, -guess); switch (gjk_status) { case GJK.eStatus.Inside: { using(EPA epa = EPA.CreateFromPool()) { EPA.eStatus epa_status = epa.Evaluate(gjk, -guess); if (epa_status != EPA.eStatus.Failed) { btVector3 w0 = new btVector3(0, 0, 0); for (U i = 0; i < epa.m_result.rank; ++i) { #region w0 += shape.Support(epa.m_result.c[i].d, 0) * epa.m_result.p[i]; { btVector3 temp1, temp2; shape.Support(ref epa.m_result.c[i].d, 0, out temp1); btVector3.Multiply(ref temp1, epa.m_result.p[i], out temp2); w0.Add(ref temp2); } #endregion } results.status = sResults.eStatus.Penetrating; results.witnesses0 = wtrs0 * w0; results.witnesses1 = wtrs0 * (w0 - epa.m_normal * epa.m_depth); results.normal = -epa.m_normal; results.distance = -epa.m_depth; return (true); } else results.status = sResults.eStatus.EPA_Failed; } } break; case GJK.eStatus.Failed: results.status = sResults.eStatus.GJK_Failed; break; default: { break; } } return (false); } }
public static void integrateTransform(btTransform curTrans,btVector3 linvel, btVector3 angvel, float timeStep,out btTransform predictedTransform) { predictedTransform = btTransform.Identity; #region predictedTransform.Origin=curTrans.Origin + linvel * timeStep; { btVector3 temp; btVector3.Multiply(ref linvel, timeStep, out temp); btVector3.Add(ref curTrans.Origin, ref temp, out predictedTransform.Origin); } #endregion //Exponential map //google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia btVector3 axis; float fAngle = angvel.Length; //limit the angular motion if (fAngle * timeStep > ANGULAR_MOTION_THRESHOLD) { fAngle = ANGULAR_MOTION_THRESHOLD / timeStep; } if (fAngle < 0.001f) { // use Taylor's expansions of sync function #region axis = angvel * (0.5f * timeStep - (timeStep * timeStep * timeStep) * (0.020833333333f) * fAngle * fAngle); { btVector3.Multiply(ref angvel, (0.5f * timeStep - (timeStep * timeStep * timeStep) * (0.020833333333f) * fAngle * fAngle), out axis); } #endregion } else { // sync(fAngle) = sin(c*fAngle)/t #region axis = angvel * ((float)Math.Sin(0.5f * fAngle * timeStep) / fAngle); btVector3.Multiply(ref angvel, ((float)Math.Sin(0.5f * fAngle * timeStep) / fAngle), out axis); #endregion } btQuaternion dorn = new btQuaternion(axis.X, axis.Y, axis.Z, (float)Math.Cos(fAngle * timeStep * 0.5f)); btQuaternion orn0 = curTrans.Rotation; btQuaternion predictedOrn; btQuaternion.Multiply(ref dorn, ref orn0, out predictedOrn); predictedOrn.normalize(); predictedTransform.Rotation = predictedOrn; }
public static bool Distance(ConvexShape shape0, btTransform wtrs0, ConvexShape shape1, btTransform wtrs1, btVector3 guess, ref sResults results) { tShape shape = new MinkowskiDiff(); Initialize(shape0, wtrs0, shape1, wtrs1, ref results, ref shape, false); using(GJK gjk = GJK.CreateFromPool()) { GJK.eStatus gjk_status = gjk.Evaluate(shape, guess); if (gjk_status == GJK.eStatus.Valid) { btVector3 w0 = new btVector3(0, 0, 0); btVector3 w1 = new btVector3(0, 0, 0); for (U i = 0; i < gjk.m_simplex.rank; ++i) { float p = gjk.m_simplex.p[i]; btVector3 temp,temp2,temp3; #region w0 += shape.Support(gjk.m_simplex.c[i].d, 0) * p; shape.Support(ref gjk.m_simplex.c[i].d, 0, out temp); btVector3.Multiply(ref temp, p, out temp2); w0.Add(ref temp2); #endregion #region w1 += shape.Support(-gjk.m_simplex.c[i].d, 1) * p; btVector3.Minus(ref gjk.m_simplex.c[i].d, out temp3); shape.Support(ref temp3, 1, out temp); btVector3.Multiply(ref temp, p, out temp2); w1.Add(ref temp2); #endregion } results.witnesses0 = wtrs0 * w0; results.witnesses1 = wtrs0 * w1; results.normal = w0 - w1; results.distance = results.normal.Length; results.normal /= results.distance > GJK_MIN_DISTANCE ? results.distance : 1; return (true); } else { results.status = gjk_status == GJK.eStatus.Inside ? sResults.eStatus.Penetrating : sResults.eStatus.GJK_Failed; return (false); } } }