public static void CalculateDiffAxisAngle(ref IndexedMatrix transform0, ref IndexedMatrix transform1, out IndexedVector3 axis, out float angle) { //IndexedMatrix dmat = GetRotateMatrix(ref transform1) * IndexedMatrix.Invert(GetRotateMatrix(ref transform0)); IndexedBasisMatrix dmat = transform1._basis * transform0._basis.Inverse(); IndexedQuaternion dorn = IndexedQuaternion.Identity; GetRotation(ref dmat, out dorn); ///floating point inaccuracy can lead to w component > 1..., which breaks dorn.Normalize(); angle = MathUtil.QuatAngle(ref dorn); axis = new IndexedVector3(dorn.X, dorn.Y, dorn.Z); //axis[3] = float(0.); //check for axis length float len = axis.LengthSquared(); if (len < MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON) { axis = new IndexedVector3(1,0,0); } else { axis.Normalize(); } }
///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version public override void GetAabb(ref IndexedMatrix trans, out IndexedVector3 aabbMin, out IndexedVector3 aabbMax) { IndexedVector3 localHalfExtents = .5f * (m_localAabbMax - m_localAabbMin); IndexedVector3 localCenter = .5f * (m_localAabbMax + m_localAabbMin); //avoid an illegal AABB when there are no children if (m_children.Count == 0) { localHalfExtents = IndexedVector3.Zero; localCenter = IndexedVector3.Zero; } float margin = GetMargin(); localHalfExtents += new IndexedVector3(margin); IndexedBasisMatrix abs_b = trans._basis.Absolute(); IndexedVector3 center = trans * localCenter; IndexedVector3 extent = new IndexedVector3(abs_b._el0.Dot(ref localHalfExtents), abs_b._el1.Dot(ref localHalfExtents), abs_b._el2.Dot(ref localHalfExtents)); aabbMin = center - extent; aabbMax = center + extent; }
//angular constraint between two different rigidbodies public JacobianEntry(IndexedVector3 jointAxis, IndexedBasisMatrix world2A, IndexedBasisMatrix world2B, IndexedVector3 inertiaInvA, IndexedVector3 inertiaInvB) : this(ref jointAxis, ref world2A, ref world2B, ref inertiaInvA, ref inertiaInvB) { }
public virtual void InternalProcessTriangleIndex(IndexedVector3[] triangle, int partId, int triangleIndex) { IndexedBasisMatrix i = new IndexedBasisMatrix(); IndexedVector3 a = triangle[0] - m_center; IndexedVector3 b = triangle[1] - m_center; IndexedVector3 c = triangle[2] - m_center; float volNeg = -Math.Abs(a.Triple(ref b, ref c)) * (1.0f / 6.0f); for (int j = 0; j < 3; j++) { for (int k = 0; k <= j; k++) { i[j, k] = i[k, j] = volNeg * (0.1f * (a[j] * a[k] + b[j] * b[k] + c[j] * c[k]) + 0.05f * (a[j] * b[k] + a[k] * b[j] + a[j] * c[k] + a[k] * c[j] + b[j] * c[k] + b[k] * c[j])); } } float i00 = -i._el0.X; float i11 = -i._el1.Y; float i22 = -i._el2.Z; i[0, 0] = i11 + i22; i[1, 1] = i22 + i00; i[2, 2] = i00 + i11; m_sum._el0 += i._el0; m_sum._el1 += i._el1; m_sum._el2 += i._el2; }
public static bool ClampNormal(ref IndexedVector3 edge, ref IndexedVector3 tri_normal_org, ref IndexedVector3 localContactNormalOnB, float correctedEdgeAngle, out IndexedVector3 clampedLocalNormal) { IndexedVector3 tri_normal = tri_normal_org; //we only have a local triangle normal, not a local contact normal . only normal in world space... //either compute the current angle all in local space, or all in world space IndexedVector3 edgeCross = IndexedVector3.Cross(edge, tri_normal).Normalized(); float curAngle = GetAngle(ref edgeCross, ref tri_normal, ref localContactNormalOnB); if (correctedEdgeAngle < 0) { if (curAngle < correctedEdgeAngle) { float diffAngle = correctedEdgeAngle - curAngle; IndexedQuaternion rotation = new IndexedQuaternion(edge, diffAngle); clampedLocalNormal = new IndexedBasisMatrix(rotation) * localContactNormalOnB; return(true); } } if (correctedEdgeAngle >= 0) { if (curAngle > correctedEdgeAngle) { float diffAngle = correctedEdgeAngle - curAngle; IndexedQuaternion rotation = new IndexedQuaternion(edge, diffAngle); clampedLocalNormal = new IndexedBasisMatrix(rotation) * localContactNormalOnB; return(true); } } clampedLocalNormal = IndexedVector3.Zero; return(false); }
///computes the exact moment of inertia and the transform from the coordinate system defined by the principal axes of the moment of inertia ///and the center of mass to the current coordinate system. "masses" points to an array of masses of the children. The resulting transform ///"principal" has to be applied inversely to all children transforms in order for the local coordinate system of the compound ///shape to be centered at the center of mass and to coincide with the principal axes. This also necessitates a correction of the world transform ///of the collision object by the principal transform. public void CalculatePrincipalAxisTransform(IList <float> masses, ref IndexedMatrix principal, out IndexedVector3 inertia) { int n = m_children.Count; float totalMass = 0; IndexedVector3 center = IndexedVector3.Zero; for (int k = 0; k < n; k++) { Debug.Assert(masses[k] > 0f); center += m_children[k].m_transform._origin * masses[k]; totalMass += masses[k]; } Debug.Assert(totalMass > 0f); center /= totalMass; principal._origin = center; IndexedBasisMatrix tensor = new IndexedBasisMatrix(); for (int k = 0; k < n; k++) { IndexedVector3 i; m_children[k].m_childShape.CalculateLocalInertia(masses[k], out i); IndexedMatrix t = m_children[k].m_transform; IndexedVector3 o = t._origin - center; //compute inertia tensor in coordinate system of compound shape IndexedBasisMatrix j = t._basis.Transpose(); j._el0 *= i.X; j._el1 *= i.Y; j._el2 *= i.Z; j = t._basis * j; //add inertia tensor tensor._el0 += j._el0; tensor._el1 += j._el1; tensor._el2 += j._el2; //tensor += j; //compute inertia tensor of pointmass at o float o2 = o.LengthSquared(); j._el0 = new IndexedVector3(o2, 0, 0); j._el1 = new IndexedVector3(0, o2, 0); j._el2 = new IndexedVector3(0, 0, o2); j._el0 += o * -o.X; j._el1 += o * -o.Y; j._el2 += o * -o.Z; //add inertia tensor of pointmass tensor._el0 += masses[k] * j._el0; tensor._el1 += masses[k] * j._el1; tensor._el2 += masses[k] * j._el2; } tensor.Diagonalize(out principal, 0.00001f, 20); inertia = new IndexedVector3(tensor._el0.X, tensor._el1.Y, tensor._el2.Z); }
//constraint on one rigidbody public JacobianEntry( IndexedBasisMatrix world2A, IndexedVector3 rel_pos1, IndexedVector3 rel_pos2, IndexedVector3 jointAxis, IndexedVector3 inertiaInvA, float massInvA) : this(ref world2A, ref rel_pos1, ref rel_pos2, ref jointAxis, ref inertiaInvA, massInvA) { }
//! Calcs the full invertion of the matrices. Useful for scaling matrices public void CalcFromFullInvert(ref IndexedMatrix trans0, ref IndexedMatrix trans1) { m_R1to0 = trans0._basis.Inverse(); m_T1to0 = m_R1to0 * (-trans0._origin); m_T1to0 += m_R1to0 * trans1._origin; m_R1to0 *= trans1._basis; CalcAbsoluteMatrix(); }
///bilateral constraint between two dynamic objects ///positive distance = separation, negative distance = penetration public static void ResolveSingleBilateral(RigidBody body1, ref IndexedVector3 pos1, RigidBody body2, ref IndexedVector3 pos2, float distance, ref IndexedVector3 normal, ref float impulse, float timeStep) { float normalLenSqr = normal.LengthSquared(); Debug.Assert(Math.Abs(normalLenSqr) < 1.1f); if (normalLenSqr > 1.1f) { impulse = 0f; return; } IndexedVector3 rel_pos1 = pos1 - body1.GetCenterOfMassPosition(); IndexedVector3 rel_pos2 = pos2 - body2.GetCenterOfMassPosition(); //this jacobian entry could be re-used for all iterations IndexedVector3 vel1 = body1.GetVelocityInLocalPoint(ref rel_pos1); IndexedVector3 vel2 = body2.GetVelocityInLocalPoint(ref rel_pos2); IndexedVector3 vel = vel1 - vel2; IndexedBasisMatrix m1 = body1.GetCenterOfMassTransform()._basis.Transpose(); IndexedBasisMatrix m2 = body2.GetCenterOfMassTransform()._basis.Transpose(); JacobianEntry jac = new JacobianEntry(m1, m2, rel_pos1, rel_pos2, normal, body1.GetInvInertiaDiagLocal(), body1.GetInvMass(), body2.GetInvInertiaDiagLocal(), body2.GetInvMass()); float jacDiagAB = jac.GetDiagonal(); float jacDiagABInv = 1f / jacDiagAB; float rel_vel = jac.GetRelativeVelocity( body1.GetLinearVelocity(), body1.GetCenterOfMassTransform()._basis.Transpose() * body1.GetAngularVelocity(), body2.GetLinearVelocity(), body2.GetCenterOfMassTransform()._basis.Transpose() * body2.GetAngularVelocity()); float a = jacDiagABInv; rel_vel = normal.Dot(ref vel); //todo: move this into proper structure float contactDamping = 0.2f; if (ONLY_USE_LINEAR_MASS) { float massTerm = 1f / (body1.GetInvMass() + body2.GetInvMass()); impulse = -contactDamping * rel_vel * massTerm; } else { float velocityImpulse = -contactDamping * rel_vel * jacDiagABInv; impulse = velocityImpulse; } }
public static void ChPosMatrToBullet(ChVector pos, ChMatrix33 <double> rA, ref BulletXNA.LinearMath.IndexedMatrix mtransform) { IndexedBasisMatrix basisA = new IndexedBasisMatrix((float)rA.nm.matrix[0, 0], (float)rA.nm.matrix[0, 1], (float)rA.nm.matrix[0, 2], (float)rA.nm.matrix[1, 0], (float)rA.nm.matrix[1, 1], (float)rA.nm.matrix[1, 2], (float)rA.nm.matrix[2, 0], (float)rA.nm.matrix[2, 1], (float)rA.nm.matrix[2, 2]); mtransform._basis = basisA; mtransform._origin = new IndexedVector3((float)pos.x, (float)pos.y, (float)pos.z); }
//! Calc the transformation relative 1 to 0. Inverts matrics by transposing public void CalcFromHomogenic(ref IndexedMatrix trans0, ref IndexedMatrix trans1) { IndexedMatrix temp_trans = trans0.Inverse(); temp_trans = temp_trans * trans1; m_T1to0 = temp_trans._origin; m_R1to0 = temp_trans._basis; CalcAbsoluteMatrix(); }
public override void SyncPosition() { ChCoordsys mcsys = this.mcontactable.GetCsysForCollisionModel(); bt_collision_object.GetWorldTransform()._origin = new IndexedVector3( (float)mcsys.pos.x, (float)mcsys.pos.y, (float)mcsys.pos.z); ChMatrix33 <double> rA = new ChMatrix33 <double>(mcsys.rot); IndexedBasisMatrix basisA = new IndexedBasisMatrix((float)rA.nm.matrix[0, 0], (float)rA.nm.matrix[0, 1], (float)rA.nm.matrix[0, 2], (float)rA.nm.matrix[1, 0], (float)rA.nm.matrix[1, 1], (float)rA.nm.matrix[1, 2], (float)rA.nm.matrix[2, 0], (float)rA.nm.matrix[2, 1], (float)rA.nm.matrix[2, 2]); bt_collision_object.GetWorldTransform()._basis = basisA; }
public static void InverseTransformPoint3x3(out IndexedVector3 outVec, ref IndexedVector3 input, ref IndexedMatrix tr) { IndexedBasisMatrix rot = tr._basis; IndexedVector3 r0 = rot._el0; IndexedVector3 r1 = rot._el1; IndexedVector3 r2 = rot._el2; float x = r0.X * input.X + r1.X * input.Y + r2.X * input.Z; float y = r0.Y * input.X + r1.Y * input.Y + r2.Y * input.Z; float z = r0.Z * input.X + r1.Z * input.Y + r2.Z * input.Z; outVec = new IndexedVector3(x, y, z); }
public static void TransformAabb(ref IndexedVector3 halfExtents, float margin, ref IndexedMatrix t, out IndexedVector3 aabbMinOut, out IndexedVector3 aabbMaxOut) { IndexedVector3 halfExtentsWithMargin = halfExtents + new IndexedVector3(margin); IndexedBasisMatrix abs_b = t._basis.Absolute(); IndexedVector3 center = t._origin; IndexedVector3 extent = new IndexedVector3(abs_b._el0.Dot(ref halfExtentsWithMargin), abs_b._el1.Dot(ref halfExtentsWithMargin), abs_b._el2.Dot(ref halfExtentsWithMargin)); aabbMinOut = center - extent; aabbMaxOut = center + extent; }
public override void ClientMoveAndDisplay(GameTime gameTime) { IndexedVector3 walkDirection = IndexedVector3.Zero; float walkVelocity = 1.1f * 4.0f; // 4 km/h -> 1.1 m/s float walkSpeed = walkVelocity * 0.001f; IndexedMatrix xform = ghostObject.GetWorldTransform(); IndexedVector3 forwardDir = xform._basis[2]; IndexedVector3 upDir = xform._basis[1]; IndexedVector3 strafeDir = xform._basis[0]; forwardDir.Normalize(); upDir.Normalize(); strafeDir.Normalize(); KeyboardState keyboardState = Keyboard.GetState(); if (keyboardState.IsKeyDown(Keys.I)) { walkDirection += forwardDir; } if (keyboardState.IsKeyDown(Keys.K)) { walkDirection -= forwardDir; } if (keyboardState.IsKeyDown(Keys.J)) { IndexedMatrix orn = ghostObject.GetWorldTransform(); orn._basis *= IndexedBasisMatrix.CreateFromAxisAngle(new IndexedVector3(0, 1, 0), 0.01f); ghostObject.SetWorldTransform(orn); } if (keyboardState.IsKeyDown(Keys.L)) { IndexedMatrix orn = ghostObject.GetWorldTransform(); orn._basis *= IndexedBasisMatrix.CreateFromAxisAngle(new IndexedVector3(0, 1, 0), -0.01f); ghostObject.SetWorldTransform(orn); } //if (key == Keys.O) //{ // playerController.Jump(); //} IndexedVector3 result = walkDirection * walkSpeed; playerController.SetWalkDirection(ref result); base.ClientMoveAndDisplay(gameTime); }
public JacobianEntry(ref IndexedVector3 jointAxis, ref IndexedBasisMatrix world2A, ref IndexedBasisMatrix world2B, ref IndexedVector3 inertiaInvA, ref IndexedVector3 inertiaInvB) { m_linearJointAxis = IndexedVector3.Zero; m_aJ = world2A * jointAxis; m_bJ = world2B * -jointAxis; m_0MinvJt = inertiaInvA * m_aJ; m_1MinvJt = inertiaInvB * m_bJ; m_Adiag = IndexedVector3.Dot(m_0MinvJt, m_aJ) + IndexedVector3.Dot(m_1MinvJt, m_bJ); Debug.Assert(m_Adiag > 0.0f); }
public override void GetAabb(ref IndexedMatrix trans, out IndexedVector3 aabbMin, out IndexedVector3 aabbMax) { IndexedVector3 halfExtents = new IndexedVector3(GetRadius()); halfExtents[m_upAxis] = GetRadius() + GetHalfHeight(); halfExtents += new IndexedVector3(GetMargin()); IndexedBasisMatrix abs_b = trans._basis.Absolute(); IndexedVector3 center = trans._origin; IndexedVector3 extent = new IndexedVector3(abs_b._el0.Dot(ref halfExtents), abs_b._el1.Dot(ref halfExtents), abs_b._el2.Dot(ref halfExtents)); aabbMin = center - extent; aabbMax = center + extent; }
public JacobianEntry( ref IndexedBasisMatrix world2A, ref IndexedVector3 rel_pos1, ref IndexedVector3 rel_pos2, ref IndexedVector3 jointAxis, ref IndexedVector3 inertiaInvA, float massInvA) { m_linearJointAxis = jointAxis; m_aJ = world2A * (rel_pos1.Cross(ref jointAxis)); m_bJ = world2A * (rel_pos2.Cross(-jointAxis)); m_0MinvJt = inertiaInvA * m_aJ; m_1MinvJt = IndexedVector3.Zero; m_Adiag = massInvA + IndexedVector3.Dot(m_0MinvJt, m_aJ); Debug.Assert(m_Adiag > 0.0f); }
public void UpdateInertiaTensor() { if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugRigidBody) { BulletGlobals.g_streamWriter.WriteLine(String.Format("[{0}] RigidBody updateInertiaTensor", (String)m_userObjectPointer)); MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "invInertiaLocal", m_invInertiaLocal); MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, m_worldTransform); MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, m_worldTransform._basis.Scaled(ref m_invInertiaLocal)); MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, m_worldTransform._basis.Transpose()); } m_invInertiaTensorWorld = m_worldTransform._basis.Scaled(ref m_invInertiaLocal) * m_worldTransform._basis.Transpose(); if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugRigidBody) { MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, m_invInertiaTensorWorld); } }
//! Apply a transform to an AABB public void ApplyTransformTransCache(ref BT_BOX_BOX_TRANSFORM_CACHE trans) { IndexedVector3 center = (m_max + m_min) * 0.5f; IndexedVector3 extends = m_max - center; // Compute new center center = trans.Transform(ref center); IndexedBasisMatrix absMatrix = trans.m_R1to0.Absolute(); IndexedVector3 textends = new IndexedVector3(extends.Dot(trans.m_R1to0.GetRow(0).Absolute()), extends.Dot(trans.m_R1to0.GetRow(1).Absolute()), extends.Dot(trans.m_R1to0.GetRow(2).Absolute())); m_min = center - textends; m_max = center + textends; }
public override void ClientMoveAndDisplay(GameTime gameTime) { IndexedVector3 walkDirection = IndexedVector3.Zero; float walkVelocity = 1.1f * 4.0f; float walkSpeed = walkVelocity * gameTime.ElapsedGameTime.Milliseconds / 1000.0f; IndexedMatrix xform = m_ghostObject.GetWorldTransform(); IndexedVector3 forwardDir = xform._basis[2]; IndexedVector3 upDir = xform._basis[1]; IndexedVector3 strafeDir = xform._basis[0]; forwardDir.Normalize(); upDir.Normalize(); strafeDir.Normalize(); KeyboardState keyboardState = Keyboard.GetState(); if (keyboardState.IsKeyDown(Keys.I)) { walkDirection += forwardDir; } if (keyboardState.IsKeyDown(Keys.K)) { walkDirection -= forwardDir; } if (keyboardState.IsKeyDown(Keys.J)) { IndexedMatrix orn = m_ghostObject.GetWorldTransform(); orn._basis *= IndexedBasisMatrix.CreateFromAxisAngle(new IndexedVector3(0, 1, 0), 0.01f); m_ghostObject.SetWorldTransform(orn); } if (keyboardState.IsKeyDown(Keys.L)) { IndexedMatrix orn = m_ghostObject.GetWorldTransform(); orn._basis *= IndexedBasisMatrix.CreateFromAxisAngle(new IndexedVector3(0, 1, 0), -0.01f); m_ghostObject.SetWorldTransform(orn); } IndexedVector3 result = walkDirection * walkSpeed; m_character.SetWalkDirection(ref result); base.ClientMoveAndDisplay(gameTime); }
public override void GetAabb(ref IndexedMatrix trans, out IndexedVector3 aabbMin, out IndexedVector3 aabbMax) { IndexedVector3 localHalfExtents = 0.5f * (m_localAabbMax - m_localAabbMin); float margin = GetMargin(); localHalfExtents += new IndexedVector3(margin); IndexedVector3 localCenter = 0.5f * (m_localAabbMax + m_localAabbMin); IndexedBasisMatrix abs_b = trans._basis.Absolute(); IndexedVector3 center = trans * localCenter; IndexedVector3 extent = new IndexedVector3(abs_b._el0.Dot(ref localHalfExtents), abs_b._el1.Dot(ref localHalfExtents), abs_b._el2.Dot(ref localHalfExtents)); aabbMin = center - extent; aabbMax = center + extent; }
public static void TransformAabb(ref IndexedVector3 localAabbMin, ref IndexedVector3 localAabbMax, float margin, ref IndexedMatrix trans, out IndexedVector3 aabbMinOut, out IndexedVector3 aabbMaxOut) { Debug.Assert(localAabbMin.X <= localAabbMax.X); Debug.Assert(localAabbMin.Y <= localAabbMax.Y); Debug.Assert(localAabbMin.Z <= localAabbMax.Z); IndexedVector3 localHalfExtents = 0.5f * (localAabbMax - localAabbMin); localHalfExtents += new IndexedVector3(margin); IndexedVector3 localCenter = 0.5f * (localAabbMax + localAabbMin); IndexedBasisMatrix abs_b = trans._basis.Absolute(); IndexedVector3 center = trans * localCenter; IndexedVector3 extent = new IndexedVector3(abs_b._el0.Dot(ref localHalfExtents), abs_b._el1.Dot(ref localHalfExtents), abs_b._el2.Dot(ref localHalfExtents)); aabbMinOut = center - extent; aabbMaxOut = center + extent; }
public override void GetAabb(ref IndexedMatrix t, out IndexedVector3 aabbMin, out IndexedVector3 aabbMax) { IndexedVector3 halfExtents = (m_localAabbMax - m_localAabbMin) * m_localScaling * 0.5f; IndexedVector3 localOrigin = IndexedVector3.Zero; localOrigin[m_upAxis] = (m_minHeight + m_maxHeight) * 0.5f; localOrigin *= m_localScaling; IndexedBasisMatrix abs_b = t._basis.Absolute(); IndexedVector3 center = t._origin; IndexedVector3 extent = new IndexedVector3(abs_b._el0.Dot(ref halfExtents), abs_b._el1.Dot(ref halfExtents), abs_b._el2.Dot(ref halfExtents)); extent += new IndexedVector3(GetMargin()); aabbMin = center - extent; aabbMax = center + extent; }
///computes the exact moment of inertia and the transform from the coordinate system defined by the principal axes of the moment of inertia ///and the center of mass to the current coordinate system. A mass of 1 is assumed, for other masses just multiply the computed "inertia" ///by the mass. The resulting transform "principal" has to be applied inversely to the mesh in order for the local coordinate system of the ///shape to be centered at the center of mass and to coincide with the principal axes. This also necessitates a correction of the world transform ///of the collision object by the principal transform. This method also computes the volume of the convex mesh. public void CalculatePrincipalAxisTransform(ref IndexedMatrix principal, out IndexedVector3 inertia, float volume) { CenterCallback centerCallback = new CenterCallback(); IndexedVector3 aabbMax = MathUtil.MAX_VECTOR; IndexedVector3 aabbMin = MathUtil.MIN_VECTOR; m_stridingMesh.InternalProcessAllTriangles(centerCallback, ref aabbMin, ref aabbMax); IndexedVector3 center = centerCallback.GetCenter(); principal._origin = center; volume = centerCallback.GetVolume(); InertiaCallback inertiaCallback = new InertiaCallback(ref center); m_stridingMesh.InternalProcessAllTriangles(inertiaCallback, ref aabbMax, ref aabbMax); IndexedBasisMatrix i = inertiaCallback.GetInertia(); i.Diagonalize(out principal, 0.00001f, 20); //i.diagonalize(principal.getBasis(), 0.00001f, 20); inertia = new IndexedVector3(i[0, 0], i[1, 1], i[2, 2]); inertia /= volume; }
//public BoxBoxDetector(BoxShape box1, BoxShape box2) //{ // m_box1 = box1; // m_box2 = box2; //} // Work in progress to copy redo the box detector to remove un-necessary allocations public static void GetClosestPoints(BoxShape box1, BoxShape box2, ref ClosestPointInput input, ManifoldResult output, IDebugDraw debugDraw, bool swapResults) { IndexedMatrix transformA = input.m_transformA; IndexedMatrix transformB = input.m_transformB; if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugBoxBoxDetector) { MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "BoxBox:GCP:transformA", transformA); MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "BoxBox:GCP:transformB", transformB); } int skip = 0; Object contact = null; IndexedVector3 normal = new IndexedVector3(); float depth = 0f; int return_code = -1; int maxc = 4; IndexedVector3 translationA = new IndexedVector3(transformA._origin); IndexedVector3 translationB = new IndexedVector3(transformB._origin); //IndexedVector3 debugExtents = new IndexedVector3(2f, 2f, 2f); IndexedVector3 box1Margin = new IndexedVector3(2f * box1.GetHalfExtentsWithMargin()); IndexedVector3 box2Margin = new IndexedVector3(2f * box2.GetHalfExtentsWithMargin()); //IndexedVector3 box1Margin = 2f * debugExtents; //IndexedVector3 box2Margin = 2f * debugExtents; IndexedBasisMatrix rotateA = transformA._basis.Transpose(); IndexedBasisMatrix rotateB = transformB._basis.Transpose(); for (int j = 0; j < 3; j++) { s_temp1[0 + 4 * j] = transformA._basis[j].X; s_temp2[0 + 4 * j] = transformB._basis[j].X; s_temp1[1 + 4 * j] = transformA._basis[j].Y; s_temp2[1 + 4 * j] = transformB._basis[j].Y; s_temp1[2 + 4 * j] = transformA._basis[j].Z; s_temp2[2 + 4 * j] = transformB._basis[j].Z; } //s_temp1[0] = rotateA._Row0.X; //s_temp1[1] = rotateA._Row0.Y; //s_temp1[2] = rotateA._Row0.Z; //s_temp1[4] = rotateA._Row1.X; //s_temp1[5] = rotateA._Row1.Y; //s_temp1[6] = rotateA._Row1.Z; //s_temp1[8] = rotateA._Row2.X; //s_temp1[9] = rotateA._Row2.X; //s_temp1[10] = rotateA._Row2.X; //s_temp2[0] = rotateB._Row0.X; //s_temp2[1] = rotateB._Row0.Y; //s_temp2[2] = rotateB._Row0.Z; //s_temp2[4] = rotateB._Row1.X; //s_temp2[5] = rotateB._Row1.Y; //s_temp2[6] = rotateB._Row1.Z; //s_temp2[8] = rotateB._Row2.X; //s_temp2[9] = rotateB._Row2.Y; //s_temp2[10] = rotateB._Row2.Z; DBoxBox2(ref translationA, s_temp1, ref box1Margin, ref translationB, s_temp2, ref box2Margin, ref normal, ref depth, ref return_code, maxc, contact, skip, output); }
public virtual void UpdateFriction(float timeStep) { //calculate the impulse, so that the wheels don't move sidewards int numWheel = GetNumWheels(); if (numWheel == 0) { return; } //m_forwardWS.resize(numWheel); //m_axle.resize(numWheel); //m_forwardImpulse.resize(numWheel); //m_sideImpulse.resize(numWheel); int numWheelsOnGround = 0; //collapse all those loops into one! for (int i = 0; i < numWheel; i++) { WheelInfo wheelInfo = m_wheelInfo[i]; RigidBody groundObject = wheelInfo.m_raycastInfo.m_groundObject as RigidBody; if (groundObject != null) { numWheelsOnGround++; } m_sideImpulse[i] = 0f; m_forwardImpulse[i] = 0f; } if (numWheelsOnGround != 4) { int ibreak = 0; } { //foreach(WheelInfo wheelInfo in m_wheelInfo) for (int i = 0; i < numWheel; ++i) { WheelInfo wheelInfo = m_wheelInfo[i]; RigidBody groundObject = wheelInfo.m_raycastInfo.m_groundObject as RigidBody; if (groundObject != null) { IndexedMatrix wheelTrans = GetWheelTransformWS(i); IndexedBasisMatrix wheelBasis0 = wheelTrans._basis; m_axle[i] = new IndexedVector3( wheelBasis0._el0[m_indexRightAxis], wheelBasis0._el1[m_indexRightAxis], wheelBasis0._el2[m_indexRightAxis]); IndexedVector3 surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS; float proj = IndexedVector3.Dot(m_axle[i], surfNormalWS); m_axle[i] -= surfNormalWS * proj; m_axle[i].Normalize(); m_forwardWS[i] = IndexedVector3.Cross(surfNormalWS, m_axle[i]); m_forwardWS[i].Normalize(); IndexedVector3 tempAxle = m_axle[i]; float tempImpulse = m_sideImpulse[i]; ContactConstraint.ResolveSingleBilateral(m_chassisBody, ref wheelInfo.m_raycastInfo.m_contactPointWS, groundObject, ref wheelInfo.m_raycastInfo.m_contactPointWS, 0f, ref tempAxle, ref tempImpulse, timeStep); m_sideImpulse[i] = (tempImpulse * sideFrictionStiffness2); } } } float sideFactor = 1f; float fwdFactor = 0.5f; bool sliding = false; { for (int wheel = 0; wheel < numWheel; wheel++) { WheelInfo wheelInfo = m_wheelInfo[wheel]; RigidBody groundObject = wheelInfo.m_raycastInfo.m_groundObject as RigidBody; float rollingFriction = 0f; if (groundObject != null) { if (wheelInfo.m_engineForce != 0.0f) { rollingFriction = wheelInfo.m_engineForce * timeStep; } else { float defaultRollingFrictionImpulse = 0f; float maxImpulse = (wheelInfo.m_brake != 0f) ? wheelInfo.m_brake : defaultRollingFrictionImpulse; IndexedVector3 tempWheel = m_forwardWS[wheel]; WheelContactPoint contactPt = new WheelContactPoint(m_chassisBody, groundObject, ref wheelInfo.m_raycastInfo.m_contactPointWS, ref tempWheel, maxImpulse); m_forwardWS[wheel] = tempWheel; rollingFriction = CalcRollingFriction(contactPt); } } //switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break) m_forwardImpulse[wheel] = 0f; m_wheelInfo[wheel].m_skidInfo = 1f; if (groundObject != null) { m_wheelInfo[wheel].m_skidInfo = 1f; float maximp = wheelInfo.m_wheelsSuspensionForce * timeStep * wheelInfo.m_frictionSlip; float maximpSide = maximp; float maximpSquared = maximp * maximpSide; m_forwardImpulse[wheel] = rollingFriction; //wheelInfo.m_engineForce* timeStep; float x = (m_forwardImpulse[wheel]) * fwdFactor; float y = (m_sideImpulse[wheel]) * sideFactor; float impulseSquared = (x * x + y * y); if (impulseSquared > maximpSquared) { sliding = true; float factor = (float)(maximp / Math.Sqrt(impulseSquared)); m_wheelInfo[wheel].m_skidInfo *= factor; } } } } if (sliding) { for (int wheel = 0; wheel < numWheel; wheel++) { if (m_sideImpulse[wheel] != 0f) { if (m_wheelInfo[wheel].m_skidInfo < 1f) { m_forwardImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo; m_sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo; } } } } // apply the impulses { for (int wheel = 0; wheel < numWheel; wheel++) { WheelInfo wheelInfo = m_wheelInfo[wheel]; IndexedVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS - m_chassisBody.GetCenterOfMassPosition(); if (m_forwardImpulse[wheel] > 5f || m_sideImpulse[wheel] > 5f) { int ibreak = 0; } if (m_forwardImpulse[wheel] != 0f) { m_chassisBody.ApplyImpulse(m_forwardWS[wheel] * (m_forwardImpulse[wheel]), rel_pos); } if (m_sideImpulse[wheel] != 0f) { RigidBody groundObject = m_wheelInfo[wheel].m_raycastInfo.m_groundObject as RigidBody; IndexedVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS - groundObject.GetCenterOfMassPosition(); IndexedVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel]; #if ROLLING_INFLUENCE_FIX // fix. It only worked if car's up was along Y - VT. IndexedVector3 vChassisWorldUp = GetRigidBody().GetCenterOfMassTransform()._basis.GetColumn(m_indexUpAxis); rel_pos -= vChassisWorldUp * (IndexedVector3.Dot(vChassisWorldUp, rel_pos) * (1.0f - wheelInfo.m_rollInfluence)); #else rel_pos[m_indexUpAxis] *= wheelInfo.m_rollInfluence; #endif m_chassisBody.ApplyImpulse(ref sideImp, ref rel_pos); //apply friction impulse on the ground IndexedVector3 temp = -sideImp; groundObject.ApplyImpulse(ref temp, ref rel_pos2); } } } }
public virtual void ProcessTriangle(IndexedVector3[] triangle, int partId, int triangleIndex) { //skip self-collisions if ((m_partIdA == partId) && (m_triangleIndexA == triangleIndex)) { return; } //skip duplicates (disabled for now) //if ((m_partIdA <= partId) && (m_triangleIndexA <= triangleIndex)) // return; //search for shared vertices and edges int numshared = 0; int[] sharedVertsA = new int[] { -1, -1, -1 }; int[] sharedVertsB = new int[] { -1, -1, -1 }; ///skip degenerate triangles float crossBSqr = IndexedVector3.Cross((triangle[1] - triangle[0]), (triangle[2] - triangle[0])).LengthSquared(); if (crossBSqr < m_triangleInfoMap.m_equalVertexThreshold) { return; } float crossASqr = IndexedVector3.Cross((m_triangleVerticesA[1] - m_triangleVerticesA[0]), (m_triangleVerticesA[2] - m_triangleVerticesA[0])).LengthSquared(); ///skip degenerate triangles if (crossASqr < m_triangleInfoMap.m_equalVertexThreshold) { return; } #if false printf("triangle A[0] = (%f,%f,%f)\ntriangle A[1] = (%f,%f,%f)\ntriangle A[2] = (%f,%f,%f)\n", m_triangleVerticesA[0].GetX(), m_triangleVerticesA[0].GetY(), m_triangleVerticesA[0].GetZ(), m_triangleVerticesA[1].GetX(), m_triangleVerticesA[1].GetY(), m_triangleVerticesA[1].GetZ(), m_triangleVerticesA[2].GetX(), m_triangleVerticesA[2].GetY(), m_triangleVerticesA[2].GetZ()); printf("partId=%d, triangleIndex=%d\n", partId, triangleIndex); printf("triangle B[0] = (%f,%f,%f)\ntriangle B[1] = (%f,%f,%f)\ntriangle B[2] = (%f,%f,%f)\n", triangle[0].GetX(), triangle[0].GetY(), triangle[0].GetZ(), triangle[1].GetX(), triangle[1].GetY(), triangle[1].GetZ(), triangle[2].GetX(), triangle[2].GetY(), triangle[2].GetZ()); #endif for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { if ((m_triangleVerticesA[i] - triangle[j]).LengthSquared() < m_triangleInfoMap.m_equalVertexThreshold) { sharedVertsA[numshared] = i; sharedVertsB[numshared] = j; numshared++; ///degenerate case if (numshared >= 3) { return; } } } ///degenerate case if (numshared >= 3) { return; } } switch (numshared) { case 0: { break; } case 1: { //shared vertex break; } case 2: { //shared edge //we need to make sure the edge is in the order V2V0 and not V0V2 so that the signs are correct if (sharedVertsA[0] == 0 && sharedVertsA[1] == 2) { sharedVertsA[0] = 2; sharedVertsA[1] = 0; int tmp = sharedVertsB[1]; sharedVertsB[1] = sharedVertsB[0]; sharedVertsB[0] = tmp; } int hash = GetHash(m_partIdA, m_triangleIndexA); TriangleInfo info = null; if (m_triangleInfoMap.ContainsKey(hash)) { info = m_triangleInfoMap[hash]; } else { info = new TriangleInfo(); m_triangleInfoMap[hash] = info; } int sumvertsA = sharedVertsA[0] + sharedVertsA[1]; int otherIndexA = 3 - sumvertsA; IndexedVector3 edge = new IndexedVector3(m_triangleVerticesA[sharedVertsA[1]] - m_triangleVerticesA[sharedVertsA[0]]); TriangleShape tA = new TriangleShape(m_triangleVerticesA[0], m_triangleVerticesA[1], m_triangleVerticesA[2]); int otherIndexB = 3 - (sharedVertsB[0] + sharedVertsB[1]); TriangleShape tB = new TriangleShape(triangle[sharedVertsB[1]], triangle[sharedVertsB[0]], triangle[otherIndexB]); //btTriangleShape tB(triangle[0],triangle[1],triangle[2]); IndexedVector3 normalA; IndexedVector3 normalB; tA.CalcNormal(out normalA); tB.CalcNormal(out normalB); edge.Normalize(); IndexedVector3 edgeCrossA = IndexedVector3.Normalize(IndexedVector3.Cross(edge, normalA)); { IndexedVector3 tmp = m_triangleVerticesA[otherIndexA] - m_triangleVerticesA[sharedVertsA[0]]; if (IndexedVector3.Dot(edgeCrossA, tmp) < 0) { edgeCrossA *= -1; } } IndexedVector3 edgeCrossB = IndexedVector3.Cross(edge, normalB).Normalized(); { IndexedVector3 tmp = triangle[otherIndexB] - triangle[sharedVertsB[0]]; if (IndexedVector3.Dot(edgeCrossB, tmp) < 0) { edgeCrossB *= -1; } } float angle2 = 0; float ang4 = 0.0f; IndexedVector3 calculatedEdge = IndexedVector3.Cross(edgeCrossA, edgeCrossB); float len2 = calculatedEdge.LengthSquared(); float correctedAngle = 0f; IndexedVector3 calculatedNormalB = normalA; bool isConvex = false; if (len2 < m_triangleInfoMap.m_planarEpsilon) { angle2 = 0.0f; ang4 = 0.0f; } else { calculatedEdge.Normalize(); IndexedVector3 calculatedNormalA = IndexedVector3.Cross(calculatedEdge, edgeCrossA); calculatedNormalA.Normalize(); angle2 = GetAngle(ref calculatedNormalA, ref edgeCrossA, ref edgeCrossB); ang4 = MathUtil.SIMD_PI - angle2; float dotA = IndexedVector3.Dot(normalA, edgeCrossB); ///@todo: check if we need some epsilon, due to floating point imprecision isConvex = (dotA < 0f); correctedAngle = isConvex ? ang4 : -ang4; IndexedQuaternion orn2 = new IndexedQuaternion(calculatedEdge, -correctedAngle); IndexedMatrix rotateMatrix = IndexedMatrix.CreateFromQuaternion(orn2); calculatedNormalB = new IndexedBasisMatrix(orn2) * normalA; } //alternatively use //IndexedVector3 calculatedNormalB2 = quatRotate(orn,normalA); switch (sumvertsA) { case 1: { IndexedVector3 edge1 = m_triangleVerticesA[0] - m_triangleVerticesA[1]; IndexedQuaternion orn = new IndexedQuaternion(edge1, -correctedAngle); IndexedVector3 computedNormalB = MathUtil.QuatRotate(orn, normalA); float bla = IndexedVector3.Dot(computedNormalB, normalB); if (bla < 0) { computedNormalB *= -1; info.m_flags |= TriangleInfoMap.TRI_INFO_V0V1_SWAP_NORMALB; } #if DEBUG_INTERNAL_EDGE if ((computedNormalB - normalB).Length() > 0.0001f) { System.Console.WriteLine("warning: normals not identical"); } #endif//DEBUG_INTERNAL_EDGE info.m_edgeV0V1Angle = -correctedAngle; if (isConvex) { info.m_flags |= TriangleInfoMap.TRI_INFO_V0V1_CONVEX; } break; } case 2: { IndexedVector3 edge1 = m_triangleVerticesA[2] - m_triangleVerticesA[0]; IndexedQuaternion orn = new IndexedQuaternion(edge1, -correctedAngle); IndexedVector3 computedNormalB = MathUtil.QuatRotate(orn, normalA); if (IndexedVector3.Dot(computedNormalB, normalB) < 0) { computedNormalB *= -1; info.m_flags |= TriangleInfoMap.TRI_INFO_V2V0_SWAP_NORMALB; } #if DEBUG_INTERNAL_EDGE if ((computedNormalB - normalB).Length() > 0.0001) { System.Console.WriteLine("warning: normals not identical"); } #endif //DEBUG_INTERNAL_EDGE info.m_edgeV2V0Angle = -correctedAngle; if (isConvex) { info.m_flags |= TriangleInfoMap.TRI_INFO_V2V0_CONVEX; } break; } case 3: { IndexedVector3 edge1 = m_triangleVerticesA[1] - m_triangleVerticesA[2]; IndexedQuaternion orn = new IndexedQuaternion(edge1, -correctedAngle); IndexedVector3 computedNormalB = MathUtil.QuatRotate(orn, normalA); if (IndexedVector3.Dot(computedNormalB, normalB) < 0) { info.m_flags |= TriangleInfoMap.TRI_INFO_V1V2_SWAP_NORMALB; computedNormalB *= -1; } #if DEBUG_INTERNAL_EDGE if ((computedNormalB - normalB).Length() > 0.0001) { System.Console.WriteLine("warning: normals not identical"); } #endif //DEBUG_INTERNAL_EDGE info.m_edgeV1V2Angle = -correctedAngle; if (isConvex) { info.m_flags |= TriangleInfoMap.TRI_INFO_V1V2_CONVEX; } break; } } break; } default: { // printf("warning: duplicate triangle\n"); break; } } }
public bool CalcPenDepth(ISimplexSolverInterface simplexSolver, ConvexShape convexA, ConvexShape convexB, ref IndexedMatrix transA, ref IndexedMatrix transB, ref IndexedVector3 v, ref IndexedVector3 pa, ref IndexedVector3 pb, IDebugDraw debugDraw) { bool check2d = convexA.IsConvex2d() && convexB.IsConvex2d(); float minProj = float.MaxValue; IndexedVector3 minNorm = IndexedVector3.Zero; IndexedVector3 minA = IndexedVector3.Zero, minB = IndexedVector3.Zero; IndexedVector3 seperatingAxisInA, seperatingAxisInB; IndexedVector3 pInA, qInB, pWorld, qWorld, w; #if USE_BATCHED_SUPPORT IndexedVector4[] supportVerticesABatch = new IndexedVector4[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2]; IndexedVector4[] supportVerticesBBatch = new IndexedVector4[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2]; IndexedVector3[] seperatingAxisInABatch = new IndexedVector3[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2]; IndexedVector3[] seperatingAxisInBBatch = new IndexedVector3[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2]; int numSampleDirections = NUM_UNITSPHERE_POINTS; for (int i = 0; i < numSampleDirections; i++) { IndexedVector3 norm = sPenetrationDirections[i]; IndexedVector3 negNorm = -norm; IndexedBasisMatrix.Multiply(ref seperatingAxisInABatch[i], ref negNorm, ref transA._basis); IndexedBasisMatrix.Multiply(ref seperatingAxisInBBatch[i], ref norm, ref transB._basis); //seperatingAxisInABatch[i] = (-norm) * transA._basis; //seperatingAxisInBBatch[i] = norm * transB._basis; } { int numPDA = convexA.GetNumPreferredPenetrationDirections(); if (numPDA > 0) { for (int i = 0; i < numPDA; i++) { IndexedVector3 norm; convexA.GetPreferredPenetrationDirection(i, out norm); IndexedBasisMatrix.Multiply(ref norm, ref transA._basis, ref norm); sPenetrationDirections[numSampleDirections] = norm; IndexedVector3 negNorm = -norm; IndexedBasisMatrix.Multiply(ref seperatingAxisInABatch[numSampleDirections], ref negNorm, ref transA._basis); IndexedBasisMatrix.Multiply(ref seperatingAxisInBBatch[numSampleDirections], ref norm, ref transB._basis); numSampleDirections++; } } } { int numPDB = convexB.GetNumPreferredPenetrationDirections(); if (numPDB > 0) { for (int i = 0; i < numPDB; i++) { IndexedVector3 norm; convexB.GetPreferredPenetrationDirection(i, out norm); IndexedBasisMatrix.Multiply(ref norm, ref transB._basis, ref norm); sPenetrationDirections[numSampleDirections] = norm; IndexedVector3 negNorm = -norm; IndexedBasisMatrix.Multiply(ref seperatingAxisInABatch[numSampleDirections], ref negNorm, ref transA._basis); IndexedBasisMatrix.Multiply(ref seperatingAxisInBBatch[numSampleDirections], ref norm, ref transB._basis); numSampleDirections++; } } } convexA.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch, supportVerticesABatch, numSampleDirections); convexB.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch, supportVerticesBBatch, numSampleDirections); for (int i = 0; i < numSampleDirections; i++) { IndexedVector3 norm = sPenetrationDirections[i]; if (check2d) { // shouldn't this be Y ? norm.Z = 0; } if (norm.LengthSquared() > 0.01f) { seperatingAxisInA = seperatingAxisInABatch[i]; seperatingAxisInB = seperatingAxisInBBatch[i]; pInA = new IndexedVector3(supportVerticesABatch[i].X, supportVerticesABatch[i].Y, supportVerticesABatch[i].Z); qInB = new IndexedVector3(supportVerticesBBatch[i].X, supportVerticesBBatch[i].Y, supportVerticesBBatch[i].Z); IndexedMatrix.Multiply(out pWorld, ref transA, ref pInA); IndexedMatrix.Multiply(out qWorld, ref transB, ref qInB); if (check2d) { // shouldn't this be Y ? pWorld.Z = 0f; qWorld.Z = 0f; } IndexedVector3.Subtract(out w, ref qWorld, ref pWorld); float delta = IndexedVector3.Dot(ref norm, ref w); //find smallest delta if (delta < minProj) { minProj = delta; minNorm = norm; minA = pWorld; minB = qWorld; } } } #else int numSampleDirections = NUM_UNITSPHERE_POINTS; { int numPDA = convexA.GetNumPreferredPenetrationDirections(); if (numPDA > 0) { for (int i = 0; i < numPDA; i++) { IndexedVector3 norm; convexA.GetPreferredPenetrationDirection(i, out norm); norm = IndexedVector3.TransformNormal(norm, transA); sPenetrationDirections[numSampleDirections] = norm; numSampleDirections++; } } } { int numPDB = convexB.GetNumPreferredPenetrationDirections(); if (numPDB > 0) { for (int i = 0; i < numPDB; i++) { IndexedVector3 norm = IndexedVector3.Zero; convexB.GetPreferredPenetrationDirection(i, out norm); norm = IndexedVector3.TransformNormal(norm, transB); sPenetrationDirections[numSampleDirections] = norm; numSampleDirections++; } } } for (int i = 0; i < numSampleDirections; i++) { IndexedVector3 norm = sPenetrationDirections[i]; if (check2d) { norm.Z = 0f; } if (norm.LengthSquared() > 0.01f) { seperatingAxisInA = IndexedVector3.TransformNormal(-norm, transA); seperatingAxisInB = IndexedVector3.TransformNormal(norm, transB); pInA = convexA.LocalGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInA); qInB = convexB.LocalGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInB); pWorld = IndexedVector3.Transform(pInA, transA); qWorld = IndexedVector3.Transform(qInB, transB); if (check2d) { pWorld.Z = 0.0f; qWorld.Z = 0.0f; } w = qWorld - pWorld; float delta = IndexedVector3.Dot(norm, w); //find smallest delta if (delta < minProj) { minProj = delta; minNorm = norm; minA = pWorld; minB = qWorld; } } } #endif //USE_BATCHED_SUPPORT //add the margins minA += minNorm * convexA.GetMarginNonVirtual(); minB -= minNorm * convexB.GetMarginNonVirtual(); //no penetration if (minProj < 0f) { return(false); } float extraSeparation = 0.5f;///scale dependent minProj += extraSeparation + (convexA.GetMarginNonVirtual() + convexB.GetMarginNonVirtual()); #if DEBUG_DRAW if (debugDraw) { IndexedVector3 color = new IndexedVector3(0, 1, 0); debugDraw.drawLine(minA, minB, color); color = new IndexedVector3(1, 1, 1); IndexedVector3 vec = minB - minA; float prj2 = IndexedVector3.Dot(minNorm, vec); debugDraw.drawLine(minA, minA + (minNorm * minProj), color); } #endif //DEBUG_DRAW GjkPairDetector gjkdet = BulletGlobals.GjkPairDetectorPool.Get(); gjkdet.Initialize(convexA, convexB, simplexSolver, null); float offsetDist = minProj; IndexedVector3 offset = minNorm * offsetDist; ClosestPointInput input = ClosestPointInput.Default(); IndexedVector3 newOrg = transA._origin + offset; IndexedMatrix displacedTrans = transA; displacedTrans._origin = newOrg; input.m_transformA = displacedTrans; input.m_transformB = transB; input.m_maximumDistanceSquared = float.MaxValue; MinkowskiIntermediateResult res = new MinkowskiIntermediateResult(); gjkdet.SetCachedSeperatingAxis(-minNorm); gjkdet.GetClosestPoints(ref input, res, debugDraw, false); float correctedMinNorm = minProj - res.m_depth; //the penetration depth is over-estimated, relax it float penetration_relaxation = 1f; minNorm *= penetration_relaxation; if (res.m_hasResult) { pa = res.m_pointInWorld - minNorm * correctedMinNorm; pb = res.m_pointInWorld; v = minNorm; #if DEBUG_DRAW if (debugDraw != null) { IndexedVector3 color = new IndexedVector3(1, 0, 0); debugDraw.drawLine(pa, pb, color); } #endif//DEBUG_DRAW } BulletGlobals.GjkPairDetectorPool.Free(gjkdet); return(res.m_hasResult); }
public virtual void GetAabbNonVirtual(ref IndexedMatrix t, ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax) { switch (m_shapeType) { case BroadphaseNativeTypes.SPHERE_SHAPE_PROXYTYPE: { SphereShape sphereShape = this as SphereShape; float radius = sphereShape.GetImplicitShapeDimensions().X;// * convexShape->getLocalScaling().getX(); float margin = radius + sphereShape.GetMarginNonVirtual(); IndexedVector3 center = t._origin; IndexedVector3 extent = new IndexedVector3(margin); aabbMin = center - extent; aabbMax = center + extent; } break; case BroadphaseNativeTypes.CYLINDER_SHAPE_PROXYTYPE: /* fall through */ case BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE: { BoxShape convexShape = this as BoxShape; float margin = convexShape.GetMarginNonVirtual(); IndexedVector3 halfExtents = convexShape.GetImplicitShapeDimensions(); halfExtents += new IndexedVector3(margin); IndexedBasisMatrix abs_b = t._basis.Absolute(); IndexedVector3 center = t._origin; IndexedVector3 extent = new IndexedVector3(abs_b._el0.Dot(ref halfExtents), abs_b._el1.Dot(ref halfExtents), abs_b._el2.Dot(ref halfExtents)); aabbMin = center - extent; aabbMax = center + extent; break; } case BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE: { TriangleShape triangleShape = (TriangleShape)this; float margin = triangleShape.GetMarginNonVirtual(); for (int i = 0; i < 3; i++) { IndexedVector3 vec = new IndexedVector3(); vec[i] = 1f; IndexedVector3 sv = LocalGetSupportVertexWithoutMarginNonVirtual(vec * t._basis); IndexedVector3 tmp = t * sv; aabbMax[i] = tmp[i] + margin; vec[i] = -1.0f; tmp = t * (LocalGetSupportVertexWithoutMarginNonVirtual(vec * t._basis)); aabbMin[i] = tmp[i] - margin; } } break; case BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE: { CapsuleShape capsuleShape = this as CapsuleShape; float r = capsuleShape.GetRadius(); IndexedVector3 halfExtents = new IndexedVector3(r); int m_upAxis = capsuleShape.GetUpAxis(); halfExtents[m_upAxis] = r + capsuleShape.GetHalfHeight(); float nvMargin = capsuleShape.GetMarginNonVirtual(); halfExtents += new IndexedVector3(nvMargin); IndexedBasisMatrix abs_b = t._basis.Absolute(); IndexedVector3 center = t._origin; IndexedVector3 extent = new IndexedVector3(abs_b._el0.Dot(ref halfExtents), abs_b._el1.Dot(ref halfExtents), abs_b._el2.Dot(ref halfExtents)); aabbMin = center - extent; aabbMax = center + extent; } break; case BroadphaseNativeTypes.CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE: case BroadphaseNativeTypes.CONVEX_HULL_SHAPE_PROXYTYPE: { PolyhedralConvexAabbCachingShape convexHullShape = (PolyhedralConvexAabbCachingShape)this; float 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); }