public override void GetAabb(ref IndexedMatrix t,out IndexedVector3 aabbMin,out IndexedVector3 aabbMax) { float fmargin = GetMargin(); IndexedVector3 margin = new IndexedVector3(fmargin); aabbMin = t._origin - margin; aabbMax = t._origin + margin; }
public static void Initialize(ConvexShape shape0,ref IndexedMatrix wtrs0, ConvexShape shape1,ref IndexedMatrix wtrs1, ref GjkEpaSolver2Results results, GjkEpaSolver2MinkowskiDiff shapeR, bool withmargins) { /* Results */ results.witnesses0 = IndexedVector3.Zero; results.witnesses1 = IndexedVector3.Zero; results.status = GjkEpaSolver2Status.Separated; /* Shape */ shapeR.m_shapes[0] = shape0; shapeR.m_shapes[1] = shape1; shapeR.m_toshape1 = wtrs1._basis.TransposeTimes(ref wtrs0._basis); shapeR.m_toshape0 = wtrs0.InverseTimes(ref wtrs1); #if DEBUG if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJK) { MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjksolver2::init::shape0", shapeR.m_toshape0); MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjksolver2::init::WTRS0", wtrs0); MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjksolver2::init::WTRS1", wtrs1); MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjksolver2::init::shape1", shapeR.m_toshape1); } #endif shapeR.EnableMargin(withmargins); }
public virtual void DrawSphere(float radius, ref IndexedMatrix transform, ref IndexedVector3 color) { IndexedVector3 start = transform._origin; IndexedVector3 xoffs = transform._basis * new IndexedVector3(radius, 0, 0); IndexedVector3 yoffs = transform._basis * new IndexedVector3(0, radius, 0); IndexedVector3 zoffs = transform._basis * new IndexedVector3(0, 0, radius); // 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); }
public DefaultMotionState(IndexedMatrix startTrans, IndexedMatrix centerOfMassOffset) { m_graphicsWorldTrans = startTrans; m_startWorldTrans = startTrans; m_centerOfMassOffset = centerOfMassOffset; m_userPointer = null; }
public GjkEpaPenetrationDepthSolver() { } // for pool public virtual bool CalcPenDepth(ISimplexSolverInterface simplexSolver, ConvexShape convexA, ConvexShape convexB, ref IndexedMatrix transA, ref IndexedMatrix transB, ref IndexedVector3 v, ref IndexedVector3 wWitnessOnA, ref IndexedVector3 wWitnessOnB, IDebugDraw debugDraw) { //float radialmargin = 0f; IndexedVector3 guessVector = (transA._origin - transB._origin); GjkEpaSolver2Results results = new GjkEpaSolver2Results(); if (GjkEpaSolver2.Penetration(convexA, ref transA, convexB, ref transB, ref 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(convexA, ref transA, convexB, ref transB, ref guessVector, ref results)) { wWitnessOnA = results.witnesses0; wWitnessOnB = results.witnesses1; v = results.normal; return false; } } return false; }
public void AddChildShape(ref IndexedMatrix localTransform, CollisionShape shape) { m_updateRevision++; //m_childTransforms.push_back(localTransform); //m_childShapes.push_back(shape); CompoundShapeChild child = new CompoundShapeChild(); child.m_transform = localTransform; child.m_childShape = shape; child.m_childShapeType = shape.GetShapeType(); child.m_childMargin = shape.GetMargin(); //extend the local aabbMin/aabbMax IndexedVector3 localAabbMin; IndexedVector3 localAabbMax; shape.GetAabb(ref localTransform, out localAabbMin, out localAabbMax); MathUtil.VectorMin(ref localAabbMin, ref m_localAabbMin); MathUtil.VectorMax(ref localAabbMax, ref m_localAabbMax); if (m_dynamicAabbTree != null) { DbvtAabbMm bounds = DbvtAabbMm.FromMM(ref localAabbMin, ref localAabbMax); int index = m_children.Count; child.m_treeNode = m_dynamicAabbTree.Insert(ref bounds, (object)index); } m_children.Add(child); }
protected virtual int SetAngularLimits(ConstraintInfo2 info, int row_offset, ref IndexedMatrix transA, ref IndexedMatrix transB, ref IndexedVector3 linVelA, ref IndexedVector3 linVelB, ref IndexedVector3 angVelA, ref IndexedVector3 angVelB) { Generic6DofConstraint d6constraint = this; int row = row_offset; //solve angular limits for (int i = 0; i < 3; i++) { if (d6constraint.GetRotationalLimitMotor(i).NeedApplyTorques()) { IndexedVector3 axis = d6constraint.GetAxis(i); int tempFlags = ((int)m_flags) >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT); SixDofFlags flags = (SixDofFlags)tempFlags; if (0 == (flags & SixDofFlags.BT_6DOF_FLAGS_CFM_NORM)) { m_angularLimits[i].m_normalCFM = info.m_solverConstraints[0].m_cfm; } if (0 == (flags & SixDofFlags.BT_6DOF_FLAGS_CFM_STOP)) { m_angularLimits[i].m_stopCFM = info.m_solverConstraints[0].m_cfm; } if (0 == (flags & SixDofFlags.BT_6DOF_FLAGS_ERP_STOP)) { m_angularLimits[i].m_stopERP = info.erp; } row += GetLimitMotorInfo2(d6constraint.GetRotationalLimitMotor(i), ref transA, ref transB, ref linVelA, ref linVelB, ref angVelA, ref angVelB, info, row, ref axis, 1, false); } } return row; }
public static bool TestInternalObjects( ref IndexedMatrix trans0, ref IndexedMatrix trans1, ref IndexedVector3 delta_c, ref IndexedVector3 axis, ConvexPolyhedron convex0, ConvexPolyhedron convex1, float dmin) { float dp = delta_c.Dot(ref axis); IndexedVector3 localAxis0; InverseTransformPoint3x3(out localAxis0, ref axis,ref trans0); IndexedVector3 localAxis1; InverseTransformPoint3x3(out localAxis1, ref axis,ref trans1); IndexedVector3 p0; BoxSupport(ref convex0.m_extents, ref localAxis0, out p0); IndexedVector3 p1; BoxSupport(ref convex1.m_extents, ref localAxis1, out p1); float Radius0 = p0.X*localAxis0.X + p0.Y*localAxis0.Y + p0.Z*localAxis0.Z; float Radius1 = p1.X*localAxis1.X + p1.Y*localAxis1.Y + p1.Z*localAxis1.Z; float MinRadius = Radius0>convex0.m_radius ? Radius0 : convex0.m_radius; float MaxRadius = Radius1>convex1.m_radius ? Radius1 : convex1.m_radius; float MinMaxRadius = MaxRadius + MinRadius; float d0 = MinMaxRadius + dp; float d1 = MinMaxRadius - dp; float depth = d0<d1 ? d0:d1; if (depth > dmin) { return false; } return true; }
public static bool Distance(ConvexShape shape0,ref IndexedMatrix wtrs0,ConvexShape shape1,ref IndexedMatrix wtrs1,ref IndexedVector3 guess,ref GjkEpaSolver2Results results) { using (GjkEpaSolver2MinkowskiDiff shape = BulletGlobals.GjkEpaSolver2MinkowskiDiffPool.Get()) using (GJK gjk = BulletGlobals.GJKPool.Get()) { Initialize(shape0, ref wtrs0, shape1, ref wtrs1, ref results, shape, false); gjk.Initialise(); GJKStatus gjk_status = gjk.Evaluate(shape, ref guess); if (gjk_status == GJKStatus.Valid) { IndexedVector3 w0 = IndexedVector3.Zero; IndexedVector3 w1 = IndexedVector3.Zero; for (uint i = 0; i < gjk.m_simplex.rank; ++i) { float p = gjk.m_simplex.p[i]; w0 += shape.Support(ref gjk.m_simplex.c[i].d, 0) * p; IndexedVector3 temp = -gjk.m_simplex.c[i].d; w1 += shape.Support(ref temp, 1) * p; } 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 { //GjkEpaSolver2Status results.status = (gjk_status == GJKStatus.Inside) ? GjkEpaSolver2Status.Penetrating : GjkEpaSolver2Status.GJK_Failed; return (false); } } }
public EntityProperties FromTransform(uint id, IndexedMatrix startTransform) { EntityProperties ret = new EntityProperties(); ID = id; Position = startTransform._origin; Rotation = startTransform.GetRotation(); return ret; }
public static void GetPlaneEquationTransformed(StaticPlaneShape plane,ref IndexedMatrix trans, out IndexedVector4 equation) { equation = new IndexedVector4(); IndexedVector3 planeNormal = plane.GetPlaneNormal(); equation.X = trans._basis.GetRow(0).Dot(ref planeNormal); equation.Y = trans._basis.GetRow(1).Dot(ref planeNormal); equation.Z = trans._basis.GetRow(2).Dot(ref planeNormal); equation.W = trans._origin.Dot(ref planeNormal) + plane.GetPlaneConstant(); }
public Generic6DofConstraint(RigidBody rbA, RigidBody rbB, ref IndexedMatrix frameInA, ref IndexedMatrix frameInB, bool useLinearReferenceFrameA) : base(TypedConstraintType.D6_CONSTRAINT_TYPE, rbA, rbB) { m_frameInA = frameInA; m_frameInB = frameInB; m_useLinearReferenceFrameA = useLinearReferenceFrameA; m_useOffsetForConstraintFrame = D6_USE_FRAME_OFFSET; m_linearLimits = new TranslationalLimitMotor(); m_angularLimits[0] = new RotationalLimitMotor(); m_angularLimits[1] = new RotationalLimitMotor(); m_angularLimits[2] = new RotationalLimitMotor(); CalculateTransforms(); }
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 void ConvexSweepTest(ConvexShape castShape, ref IndexedMatrix convexFromWorld, ref IndexedMatrix convexToWorld, ConvexResultCallback resultCallback, float allowedCcdPenetration) { IndexedMatrix convexFromTrans = convexFromWorld; IndexedMatrix convexToTrans = convexToWorld; IndexedVector3 castShapeAabbMin; IndexedVector3 castShapeAabbMax; /* Compute AABB that encompasses angular movement */ IndexedVector3 linVel, angVel; TransformUtil.CalculateVelocity(ref convexFromTrans, ref convexToTrans, 1.0f, out linVel, out angVel); // FIXME MAN check this - should be a get/set rotation call, basis copy like this may break with scale? IndexedMatrix R = IndexedMatrix.Identity; R.SetRotation(convexFromTrans.GetRotation()); castShape.CalculateTemporalAabb(ref R, ref linVel, ref angVel, 1.0f, out castShapeAabbMin, out castShapeAabbMax); /// go over all objects, and if the ray intersects their aabb + cast shape aabb, // do a ray-shape query using convexCaster (CCD) for (int i = 0; i < m_overlappingObjects.Count; i++) { CollisionObject collisionObject = m_overlappingObjects[i]; //only perform raycast if filterMask matches if (resultCallback.NeedsCollision(collisionObject.GetBroadphaseHandle())) { //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); IndexedVector3 collisionObjectAabbMin; IndexedVector3 collisionObjectAabbMax; IndexedMatrix t = collisionObject.GetWorldTransform(); collisionObject.GetCollisionShape().GetAabb(ref t, out collisionObjectAabbMin, out collisionObjectAabbMax); AabbUtil2.AabbExpand(ref collisionObjectAabbMin, ref collisionObjectAabbMax, ref castShapeAabbMin, ref castShapeAabbMax); float hitLambda = 1f; //could use resultCallback.m_closestHitFraction, but needs testing IndexedVector3 hitNormal; if (AabbUtil2.RayAabb(convexFromWorld._origin, convexToWorld._origin, ref collisionObjectAabbMin, ref collisionObjectAabbMax, ref hitLambda, out hitNormal)) { IndexedMatrix wt = collisionObject.GetWorldTransform(); CollisionWorld.ObjectQuerySingle(castShape, ref convexFromTrans, ref convexToTrans, collisionObject, collisionObject.GetCollisionShape(), ref wt, resultCallback, allowedCcdPenetration); } } } }
public virtual void DrawBox(ref IndexedVector3 bbMin, ref IndexedVector3 bbMax, ref IndexedMatrix trans, ref IndexedVector3 color) { DrawLine(trans * bbMin, trans * new IndexedVector3(bbMax.X, bbMin.Y, bbMin.Z), color); DrawLine(trans * new IndexedVector3(bbMax.X, bbMin.Y, bbMin.Z), trans * new IndexedVector3(bbMax.X, bbMax.Y, bbMin.Z), color); DrawLine(trans * new IndexedVector3(bbMax.X, bbMax.Y, bbMin.Z), trans * new IndexedVector3(bbMin.X, bbMax.Y, bbMin.Z), color); DrawLine(trans * new IndexedVector3(bbMin.X, bbMax.Y, bbMin.Z), trans * bbMin, color); DrawLine(trans * bbMin, trans * new IndexedVector3(bbMin.X, bbMin.Y, bbMax.Z), color); DrawLine(trans * new IndexedVector3(bbMax.X, bbMin.Y, bbMin.Z), trans * new IndexedVector3(bbMax.X, bbMin.Y, bbMax.Z), color); DrawLine(trans * new IndexedVector3(bbMax.X, bbMax.Y, bbMin.Z), trans * bbMax, color); DrawLine(trans * new IndexedVector3(bbMin.X, bbMax.Y, bbMin.Z), trans * new IndexedVector3(bbMin.X, bbMax.Y, bbMax.Z), color); DrawLine(trans * new IndexedVector3(bbMin.X, bbMin.Y, bbMax.Z), trans * new IndexedVector3(bbMax.X, bbMin.Y, bbMax.Z), color); DrawLine(trans * new IndexedVector3(bbMax.X, bbMin.Y, bbMax.Z), trans * bbMax, color); DrawLine(trans * bbMax, trans * new IndexedVector3(bbMin.X, bbMax.Y, bbMax.Z), color); DrawLine(trans * new IndexedVector3(bbMin.X, bbMax.Y, bbMax.Z), trans * new IndexedVector3(bbMin.X, bbMin.Y, bbMax.Z), color); }
public Generic6DofConstraint(RigidBody rbB, ref IndexedMatrix frameInB, bool useLinearReferenceFrameB) : base(TypedConstraintType.D6_CONSTRAINT_TYPE, GetFixedBody(), rbB) { m_frameInB = frameInB; m_useLinearReferenceFrameA = useLinearReferenceFrameB; m_useOffsetForConstraintFrame = D6_USE_FRAME_OFFSET; m_linearLimits = new TranslationalLimitMotor(); m_angularLimits[0] = new RotationalLimitMotor(); m_angularLimits[1] = new RotationalLimitMotor(); m_angularLimits[2] = new RotationalLimitMotor(); ///not providing rigidbody A means implicitly using worldspace for body A m_frameInA = rbB.GetCenterOfMassTransform() * m_frameInB; CalculateTransforms(); }
public virtual void DrawCapsule(float radius, float halfHeight, int upAxis, ref IndexedMatrix transform, ref IndexedVector3 color) { IndexedVector3 capStart = IndexedVector3.Zero;; capStart[upAxis] = -halfHeight; IndexedVector3 capEnd = IndexedVector3.Zero; capEnd[upAxis] = halfHeight; // Draw the ends { IndexedMatrix childTransform = transform; childTransform._origin = transform * capStart; DrawSphere(radius, ref childTransform, ref color); } { IndexedMatrix childTransform = transform; childTransform._origin = transform * capEnd; DrawSphere(radius, ref childTransform, ref color); } // Draw some additional lines IndexedVector3 start = transform._origin; capStart[(upAxis + 1) % 3] = radius; capEnd[(upAxis + 1) % 3] = radius; DrawLine(start + transform._basis * capStart, start + transform._basis * capEnd, color); capStart[(upAxis + 1) % 3] = -radius; capEnd[(upAxis + 1) % 3] = -radius; DrawLine(start + transform._basis * capStart, start + transform._basis * capEnd, color); capStart[(upAxis + 2) % 3] = radius; capEnd[(upAxis + 2) % 3] = radius; DrawLine(start + transform._basis * capStart, start + transform._basis * capEnd, color); capStart[(upAxis + 2) % 3] = -radius; capEnd[(upAxis + 2) % 3] = -radius; DrawLine(start + transform._basis * capStart, start + transform._basis * capEnd, color); }
public static void ClipHullAgainstHull(ref IndexedVector3 separatingNormal1, ConvexPolyhedron hullA, ConvexPolyhedron hullB, ref IndexedMatrix transA, ref IndexedMatrix transB, float minDist, float maxDist, IDiscreteCollisionDetectorInterfaceResult resultOut) { IndexedVector3 separatingNormal = separatingNormal1.Normalized(); IndexedVector3 c0 = transA * hullA.m_localCenter; IndexedVector3 c1 = transB * hullB.m_localCenter; IndexedVector3 DeltaC2 = c0 - c1; float curMaxDist = maxDist; int closestFaceB = -1; float dmax = float.MinValue; { for (int face = 0; face < hullB.m_faces.Count; face++) { IndexedVector3 Normal = new IndexedVector3(hullB.m_faces[face].m_plane[0], hullB.m_faces[face].m_plane[1], hullB.m_faces[face].m_plane[2]); IndexedVector3 WorldNormal = transB._basis * Normal; float d = IndexedVector3.Dot(WorldNormal, separatingNormal); if (d > dmax) { dmax = d; closestFaceB = face; } } } // setup initial clip face (minimizing face from hull B) ObjectArray<IndexedVector3> worldVertsB1 = new ObjectArray<IndexedVector3>(); { Face polyB = hullB.m_faces[closestFaceB]; int numVertices = polyB.m_indices.Count; for (int e0 = 0; e0 < numVertices; e0++) { IndexedVector3 b = hullB.m_vertices[polyB.m_indices[e0]]; // check this to see if it is transposed version worldVertsB1.Add(transB * b); } } if (closestFaceB >= 0) { ClipFaceAgainstHull(ref separatingNormal, hullA, ref transA, worldVertsB1, minDist, maxDist, resultOut); } }
public override void GetAabb(ref IndexedMatrix t, out IndexedVector3 aabbMin, out IndexedVector3 aabbMax) { #if true base.GetAabb(ref t,out aabbMin,out aabbMax); #else aabbMin = MathUtil.MAX_VECTOR; aabbMax = MathUtil.MIN_VECTOR; //just transform the vertices in worldspace, and take their AABB for (int i=0;i<m_numVertices;i++) { IndexedVector3 worldVertex = IndexedVector3.Transformt(m_vertices[i],t); MathUtil.vectorMin(ref worldVertex, ref aabbMin); MathUtil.vectorMin(ref worldVertex,ref aabbMax); } #endif }
public static IndexedVector3 GimInertiaAddTransformed(ref IndexedVector3 source_inertia, ref IndexedVector3 added_inertia, ref IndexedMatrix transform) { // btMatrix3x3 rotatedTensor = transform.getBasis().scaled(added_inertia) * transform.getBasis().transpose(); // float x2 = transform.getOrigin()[0]; // x2*= x2; // float y2 = transform.getOrigin()[1]; // y2*= y2; // float z2 = transform.getOrigin()[2]; // z2*= z2; // float ix = rotatedTensor[0][0]*(y2+z2); // float iy = rotatedTensor[1][1]*(x2+z2); // float iz = rotatedTensor[2][2]*(x2+y2); // return btVector3(source_inertia[0]+ix,source_inertia[1]+iy,source_inertia[2] + iz); return IndexedVector3.Zero; }
public virtual void DrawCone(float radius, float height, int upAxis, ref IndexedMatrix transform, ref IndexedVector3 color) { IndexedVector3 start = transform._origin; IndexedVector3 offsetHeight = IndexedVector3.Zero; offsetHeight[upAxis] = height * 0.5f; IndexedVector3 offsetRadius = IndexedVector3.Zero; offsetRadius[(upAxis + 1) % 3] = radius; IndexedVector3 offset2Radius = IndexedVector3.Zero; offsetRadius[(upAxis + 2) % 3] = radius; DrawLine(start + transform._basis * offsetHeight, start + transform._basis * -offsetHeight + offsetRadius, color); DrawLine(start + transform._basis * offsetHeight, start + transform._basis * -offsetHeight - offsetRadius, color); DrawLine(start + transform._basis * offsetHeight, start + transform._basis * -offsetHeight + offset2Radius, color); DrawLine(start + transform._basis * offsetHeight, start + transform._basis * -offsetHeight - offset2Radius, color); }
///calculateTemporalAabb calculates the enclosing aabb for the moving object over interval [0..timeStep) ///result is conservative public void CalculateTemporalAabb(ref IndexedMatrix curTrans,ref IndexedVector3 linvel,ref IndexedVector3 angvel,float timeStep, out IndexedVector3 temporalAabbMin,out IndexedVector3 temporalAabbMax) { //start with static aabb GetAabb(ref curTrans,out temporalAabbMin,out temporalAabbMax); float temporalAabbMaxx = temporalAabbMax.X; float temporalAabbMaxy = temporalAabbMax.Y; float temporalAabbMaxz = temporalAabbMax.Z; float temporalAabbMinx = temporalAabbMin.X; float temporalAabbMiny = temporalAabbMin.Y; float temporalAabbMinz = temporalAabbMin.Z; // add linear motion IndexedVector3 linMotion = linvel*timeStep; ///@todo: simd would have a vector max/min operation, instead of per-element access if (linMotion.X > 0f) temporalAabbMaxx += linMotion.X; else temporalAabbMinx += linMotion.X; if (linMotion.Y > 0f) temporalAabbMaxy += linMotion.Y; else temporalAabbMiny += linMotion.Y; if (linMotion.Z > 0f) temporalAabbMaxz += linMotion.Z; else temporalAabbMinz += linMotion.Z; //add conservative angular motion float angularMotion = angvel.Length() * GetAngularMotionDisc() * timeStep; IndexedVector3 angularMotion3d = new IndexedVector3(angularMotion); temporalAabbMin = new IndexedVector3(temporalAabbMinx,temporalAabbMiny,temporalAabbMinz); temporalAabbMax = new IndexedVector3(temporalAabbMaxx,temporalAabbMaxy,temporalAabbMaxz); temporalAabbMin -= angularMotion3d; temporalAabbMax += angularMotion3d; }
public static bool TestSepAxis(ConvexPolyhedron hullA, ConvexPolyhedron hullB, ref IndexedMatrix transA, ref IndexedMatrix transB, ref IndexedVector3 sep_axis, out float depth) { float Min0, Max0; float Min1, Max1; hullA.Project(ref transA, ref sep_axis, out Min0, out Max0); hullB.Project(ref transB, ref sep_axis, out Min1, out Max1); if (Max0 < Min1 || Max1 < Min0) { depth = 0; return false; } float d0 = Max0 - Min1; Debug.Assert(d0 >= 0.0f); float d1 = Max1 - Min0; Debug.Assert(d1 >= 0.0f); depth = d0 < d1 ? d0 : d1; return true; }
public virtual void DrawBox(ref IndexedVector3 bbMin, ref IndexedVector3 bbMax, ref IndexedMatrix trans, ref IndexedVector3 color) { DrawLine(trans * bbMin, trans * new IndexedVector3(bbMax.X, bbMin.Y, bbMin.Z), color); DrawLine(trans * new IndexedVector3(bbMax.X, bbMin.Y, bbMin.Z), trans * new IndexedVector3(bbMax.X, bbMax.Y, bbMin.Z), color); DrawLine(trans * new IndexedVector3(bbMax.X, bbMax.Y, bbMin.Z), trans * new IndexedVector3(bbMin.X, bbMax.Y, bbMin.Z), color); DrawLine(trans * new IndexedVector3(bbMin.X, bbMax.Y, bbMin.Z), trans * bbMin, color); DrawLine(trans * bbMin, trans * new IndexedVector3(bbMin.X, bbMin.Y, bbMax.Z), color); DrawLine(trans * new IndexedVector3(bbMax.X, bbMin.Y, bbMin.Z), trans * new IndexedVector3(bbMax.X, bbMin.Y, bbMax.Z), color); DrawLine(trans * new IndexedVector3(bbMax.X, bbMax.Y, bbMin.Z), trans * bbMax, color); DrawLine(trans * new IndexedVector3(bbMin.X, bbMax.Y, bbMin.Z), trans * new IndexedVector3(bbMin.X, bbMax.Y, bbMax.Z), color); DrawLine(trans * new IndexedVector3(bbMin.X, bbMin.Y, bbMax.Z), trans * new IndexedVector3(bbMax.X, bbMin.Y, bbMax.Z), color); DrawLine(trans * new IndexedVector3(bbMax.X, bbMin.Y, bbMax.Z), trans * bbMax, color); DrawLine(trans * bbMax, trans * new IndexedVector3(bbMin.X, bbMax.Y, bbMax.Z), color); DrawLine(trans * new IndexedVector3(bbMin.X, bbMax.Y, bbMax.Z), trans * new IndexedVector3(bbMin.X, bbMin.Y, bbMax.Z), color); }
public static bool FindSeparatingAxis(ConvexPolyhedron hullA, ConvexPolyhedron hullB, ref IndexedMatrix transA, ref IndexedMatrix transB, out IndexedVector3 sep) { gActualSATPairTests++; // dummy value to satisfy exit points. sep = new IndexedVector3(0, 1, 0); #if TEST_INTERNAL_OBJECTS IndexedVector3 c0 = transA * hullA.m_localCenter; IndexedVector3 c1 = transB * hullB.m_localCenter; IndexedVector3 DeltaC2 = c0 - c1; #endif float dmin = float.MaxValue; int curPlaneTests = 0; int numFacesA = hullA.m_faces.Count; // Test normals from hullA for (int i = 0; i < numFacesA; i++) { IndexedVector3 Normal = new IndexedVector3(hullA.m_faces[i].m_plane[0], hullA.m_faces[i].m_plane[1], hullA.m_faces[i].m_plane[2]); IndexedVector3 faceANormalWS = transA._basis * Normal; curPlaneTests++; #if TEST_INTERNAL_OBJECTS gExpectedNbTests++; if(gUseInternalObject && !TestInternalObjects(ref transA,ref transB,ref DeltaC2, ref faceANormalWS, hullA, hullB, dmin)) continue; gActualNbTests++; #endif float d; if (!TestSepAxis(hullA, hullB, ref transA, ref transB, ref faceANormalWS, out d)) return false; if (d < dmin) { dmin = d; sep = faceANormalWS; } } int numFacesB = hullB.m_faces.Count; // Test normals from hullB for (int i = 0; i < numFacesB; i++) { IndexedVector3 Normal = new IndexedVector3(hullB.m_faces[i].m_plane[0], hullB.m_faces[i].m_plane[1], hullB.m_faces[i].m_plane[2]); IndexedVector3 WorldNormal = transB._basis * Normal; curPlaneTests++; #if TEST_INTERNAL_OBJECTS gExpectedNbTests++; if(gUseInternalObject && !TestInternalObjects(ref transA,ref transB,ref DeltaC2, ref WorldNormal, hullA, hullB, dmin)) continue; gActualNbTests++; #endif float d; if (!TestSepAxis(hullA, hullB, ref transA, ref transB, ref WorldNormal, out d)) return false; if (d < dmin) { dmin = d; sep = WorldNormal; } } IndexedVector3 edgeAstart, edgeAend, edgeBstart, edgeBend; int curEdgeEdge = 0; // Test edges for (int e0 = 0; e0 < hullA.m_uniqueEdges.Count; e0++) { IndexedVector3 edge0 = hullA.m_uniqueEdges[e0]; IndexedVector3 WorldEdge0 = transA._basis * edge0; for (int e1 = 0; e1 < hullB.m_uniqueEdges.Count; e1++) { IndexedVector3 edge1 = hullB.m_uniqueEdges[e1]; IndexedVector3 WorldEdge1 = transB._basis * edge1; IndexedVector3 Cross = IndexedVector3.Cross(WorldEdge0, WorldEdge1); curEdgeEdge++; if (!MathUtil.IsAlmostZero(ref Cross)) { Cross.Normalize(); #if TEST_INTERNAL_OBJECTS gExpectedNbTests++; if(gUseInternalObject && !TestInternalObjects(ref transA,ref transB,ref DeltaC2, ref Cross, hullA, hullB, dmin)) continue; gActualNbTests++; #endif float dist; if (!TestSepAxis(hullA, hullB, ref transA, ref transB, ref Cross, out dist)) { return false; } if (dist < dmin) { dmin = dist; sep = Cross; } } } } IndexedVector3 deltaC = transB._origin - transA._origin; if ((IndexedVector3.Dot(deltaC, sep)) > 0.0f) { sep = -sep; } return true; }
public virtual void DrawPlane(ref IndexedVector3 planeNormal, float planeConst, ref IndexedMatrix transform, ref IndexedVector3 color) { IndexedVector3 planeOrigin = planeNormal * planeConst; IndexedVector3 vec0, vec1; TransformUtil.PlaneSpace1(ref planeNormal, out vec0, out vec1); float vecLen = 100f; IndexedVector3 pt0 = planeOrigin + vec0 * vecLen; IndexedVector3 pt1 = planeOrigin - vec0 * vecLen; IndexedVector3 pt2 = planeOrigin + vec1 * vecLen; IndexedVector3 pt3 = planeOrigin - vec1 * vecLen; DrawLine(transform * pt0, transform * pt1, color); DrawLine(transform * pt2, transform * pt3, color); }
public static void ClipFaceAgainstHull(ref IndexedVector3 separatingNormal, ConvexPolyhedron hullA, ref IndexedMatrix transA, ObjectArray<IndexedVector3> worldVertsB1, float minDist, float maxDist, IDiscreteCollisionDetectorInterfaceResult resultOut) { ObjectArray<IndexedVector3> worldVertsB2 = new ObjectArray<IndexedVector3>(); ObjectArray<IndexedVector3> pVtxIn = worldVertsB1; ObjectArray<IndexedVector3> pVtxOut = worldVertsB2; pVtxOut.Capacity = pVtxIn.Count; int closestFaceA = -1; { float dmin = float.MaxValue; for (int face = 0; face < hullA.m_faces.Count; face++) { IndexedVector3 Normal = new IndexedVector3(hullA.m_faces[face].m_plane[0], hullA.m_faces[face].m_plane[1], hullA.m_faces[face].m_plane[2]); IndexedVector3 faceANormalWS = transA._basis * Normal; float d = IndexedVector3.Dot(faceANormalWS, separatingNormal); if (d < dmin) { dmin = d; closestFaceA = face; } } } if (closestFaceA < 0) return; Face polyA = hullA.m_faces[closestFaceA]; // clip polygon to back of planes of all faces of hull A that are adjacent to witness face int numContacts = pVtxIn.Count; int numVerticesA = polyA.m_indices.Count; for (int e0 = 0; e0 < numVerticesA; e0++) { IndexedVector3 a = hullA.m_vertices[polyA.m_indices[e0]]; IndexedVector3 b = hullA.m_vertices[polyA.m_indices[(e0 + 1) % numVerticesA]]; IndexedVector3 edge0 = a - b; IndexedVector3 WorldEdge0 = transA._basis * edge0; IndexedVector3 worldPlaneAnormal1 = transA._basis * new IndexedVector3(polyA.m_plane[0], polyA.m_plane[1], polyA.m_plane[2]); IndexedVector3 planeNormalWS1 = -WorldEdge0.Cross(worldPlaneAnormal1);//.cross(WorldEdge0); IndexedVector3 worldA1 = transA * a; float planeEqWS1 = -worldA1.Dot(planeNormalWS1); //int otherFace=0; #if BLA1 int otherFace = polyA.m_connectedFaces[e0]; btVector3 localPlaneNormal (hullA.m_faces[otherFace].m_plane[0],hullA.m_faces[otherFace].m_plane[1],hullA.m_faces[otherFace].m_plane[2]); btScalar localPlaneEq = hullA.m_faces[otherFace].m_plane[3]; btVector3 planeNormalWS = transA.getBasis()*localPlaneNormal; btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin()); #else IndexedVector3 planeNormalWS = planeNormalWS1; float planeEqWS=planeEqWS1; #endif //clip face ClipFace(pVtxIn, pVtxOut, ref planeNormalWS, planeEqWS); //btSwap(pVtxIn,pVtxOut); ObjectArray<IndexedVector3> temp = pVtxIn; pVtxIn = pVtxOut; pVtxOut = temp; pVtxOut.Clear(); } //#define ONLY_REPORT_DEEPEST_POINT IndexedVector3 point; // only keep points that are behind the witness face { IndexedVector3 localPlaneNormal = new IndexedVector3(polyA.m_plane[0], polyA.m_plane[1], polyA.m_plane[2]); float localPlaneEq = polyA.m_plane[3]; IndexedVector3 planeNormalWS = transA._basis * localPlaneNormal; float planeEqWS = localPlaneEq - IndexedVector3.Dot(planeNormalWS, transA._origin); for (int i = 0; i < pVtxIn.Count; i++) { float depth = IndexedVector3.Dot(planeNormalWS, pVtxIn[i]) + planeEqWS; if (depth <= minDist) { // printf("clamped: depth=%f to minDist=%f\n",depth,minDist); depth = minDist; } if (depth <= maxDist && depth >= minDist) { IndexedVector3 point2 = pVtxIn[i]; #if ONLY_REPORT_DEEPEST_POINT curMaxDist = depth; #else #if false if (depth<-3) { printf("error in btPolyhedralContactClipping depth = %f\n", depth); printf("likely wrong separatingNormal passed in\n"); } #endif resultOut.AddContactPoint(ref separatingNormal, ref point2, depth); #endif } } } #if ONLY_REPORT_DEEPEST_POINT if (curMaxDist<maxDist) { resultOut.AddContactPoint(ref separatingNormal,ref point,curMaxDist); } #endif //ONLY_REPORT_DEEPEST_POINT }
public static bool FindSeparatingAxis(ConvexPolyhedron hullA, ConvexPolyhedron hullB, IndexedMatrix transA, IndexedMatrix transB, out IndexedVector3 sep) { return FindSeparatingAxis(hullA, hullB, ref transA, ref transB, out sep); }
public virtual void DrawSphere(ref IndexedVector3 p, float radius, ref IndexedVector3 color) { IndexedMatrix tr = IndexedMatrix.CreateTranslation(p); DrawSphere(radius, ref tr, ref color); }
public static void ClipFaceAgainstHull(IndexedVector3 separatingNormal, ConvexPolyhedron hullA, IndexedMatrix transA, ObjectArray<IndexedVector3> worldVertsB1, float minDist, float maxDist, IDiscreteCollisionDetectorInterfaceResult resultOut) { ClipFaceAgainstHull(ref separatingNormal, hullA, ref transA, worldVertsB1, minDist, maxDist, resultOut); }
public static void ClipHullAgainstHull(IndexedVector3 separatingNormal, ConvexPolyhedron hullA, ConvexPolyhedron hullB, IndexedMatrix transA, IndexedMatrix transB, float minDist, float maxDist, IDiscreteCollisionDetectorInterfaceResult resultOut) { ClipHullAgainstHull(ref separatingNormal, hullA, hullB, ref transA, ref transB, minDist, maxDist, resultOut); }
///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); }
///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; }
public void UpdateChildTransform(int childIndex, ref IndexedMatrix newChildTransform, bool shouldRecalculateLocalAabb) { m_children[childIndex].m_transform = newChildTransform; if (m_dynamicAabbTree != null) { ///update the dynamic aabb tree IndexedVector3 localAabbMin; IndexedVector3 localAabbMax; m_children[childIndex].m_childShape.GetAabb(ref newChildTransform, out localAabbMin, out localAabbMax); DbvtAabbMm bounds = DbvtAabbMm.FromMM(ref localAabbMin, ref localAabbMax); //int index = m_children.Count - 1; m_dynamicAabbTree.Update(m_children[childIndex].m_treeNode, ref bounds); } if (shouldRecalculateLocalAabb) { RecalculateLocalAabb(); } }
/**@brief diagonalizes this matrix by the Jacobi method. * @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original * coordinate system, i.e., old_this = rot * new_this * rot^T. * @param threshold See iteration * @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied * by the sum of the absolute values of the diagonal, or when maxSteps have been executed. * * Note that this matrix is assumed to be symmetric. */ public void Diagonalize(out IndexedMatrix rot, float threshold, int maxSteps) { rot = IndexedMatrix.Identity; for (int step = maxSteps; step > 0; step--) { // find off-diagonal element [p][q] with largest magnitude int p = 0; int q = 1; int r = 2; float max = Math.Abs(this[0][1]); float v = Math.Abs(this[0][2]); if (v > max) { q = 2; r = 1; max = v; } v = Math.Abs(this[1][2]); if (v > max) { p = 1; q = 2; r = 0; max = v; } float t = threshold * (Math.Abs(this[0][0]) + Math.Abs(this[1][1]) + Math.Abs(this[2][2])); if (max <= t) { if (max <= MathUtil.SIMD_EPSILON * t) { return; } step = 1; } // compute Jacobi rotation J which leads to a zero for element [p][q] float mpq = this[p][q]; float theta = (this[q][q] - this[p][p]) / (2 * mpq); float theta2 = theta * theta; float cos; float sin; if (theta2 * theta2 < (10.0f / MathUtil.SIMD_EPSILON)) { t = (theta >= 0.0f) ? (1.0f / (float)(theta + (float)Math.Sqrt(1 + theta2))) : (1 / (theta - (float)Math.Sqrt(1 + theta2))); cos = 1.0f / (float)Math.Sqrt(1 + t * t); sin = cos * t; } else { // approximation for large theta-value, i.e., a nearly diagonal matrix t = 1 / (theta * (2 + 0.5f / theta2)); cos = 1 - 0.5f * t * t; sin = cos * t; } // apply rotation to matrix (this = J^T * this * J) this[p, q] = 0; this[q, p] = 0; this[p, p] -= t * mpq; this[q, q] += t * mpq; float mrp = this[r][p]; float mrq = this[r][q]; this[r, p] = this[p, r] = cos * mrp - sin * mrq; this[r, q] = this[q, r] = cos * mrq + sin * mrp; // apply rotation to rot (rot = rot * J) for (int i = 0; i < 3; i++) { mrp = this[i, p]; mrq = this[i, q]; this[i, p] = cos * mrp - sin * mrq; this[i, q] = cos * mrq + sin * mrp; } } }
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); }
public override void GetAabb(ref IndexedMatrix t, out IndexedVector3 aabbMin, out IndexedVector3 aabbMax) { aabbMin = MathUtil.MIN_VECTOR; aabbMax = MathUtil.MAX_VECTOR; }