///<summary> /// Concatenates a rigid transform with another rigid transform. ///</summary> ///<param name="a">The first rigid transform.</param> ///<param name="b">The second rigid transform.</param> ///<param name="combined">Concatenated rigid transform.</param> public static void Multiply(ref RigidTransform a, ref RigidTransform b, out RigidTransform combined) { Vector3f intermediate; Vector3f.Transform(ref a.Position, ref b.Orientation, out intermediate); Vector3f.Add(ref intermediate, ref b.Position, out combined.Position); Quaternion.Concatenate(ref a.Orientation, ref b.Orientation, out combined.Orientation); }
public override void SetWorldTransformPhysics(Vector3f trans, Quaternion rot, Vector3f scl) { base.SetWorldTransformPhysics(trans, rot, scl); foreach (GameObject gi in children) { gi.SetWorldTransformPhysics(trans.Add(gi.GetLocalTranslation()), rot.Multiply(gi.GetLocalRotation()), scl.Multiply(gi.GetLocalScale())); } }
protected internal override void UpdateJacobiansAndVelocityBias() { //Transform the anchors and offsets into world space. Vector3f offsetA, offsetB; Vector3f.Transform(ref LocalAnchorA, ref ConnectionA.Orientation, out offsetA); Vector3f.Transform(ref LocalAnchorB, ref ConnectionB.Orientation, out offsetB); Vector3f anchorA, anchorB; Vector3f.Add(ref ConnectionA.Position, ref offsetA, out anchorA); Vector3f.Add(ref ConnectionB.Position, ref offsetB, out anchorB); //Compute the distance. Vector3f separation; Vector3f.Subtract(ref anchorB, ref anchorA, out separation); float currentDistance = separation.Length; //Compute jacobians Vector3f linearA; #if !WINDOWS linearA = new Vector3f(); #endif if (currentDistance > MathHelper.Epsilon) { linearA.X = separation.X / currentDistance; linearA.Y = separation.Y / currentDistance; linearA.Z = separation.Z / currentDistance; velocityBias = new Vector3f(errorCorrectionFactor * (currentDistance - distance), 0, 0); } else { velocityBias = new Vector3f(); linearA = new Vector3f(); } Vector3f angularA, angularB; Vector3f.Cross(ref offsetA, ref linearA, out angularA); //linearB = -linearA, so just swap the cross product order. Vector3f.Cross(ref linearA, ref offsetB, out angularB); //Put all the 1x3 jacobians into a 3x3 matrix representation. linearJacobianA = new Matrix3f { M11 = linearA.X, M12 = linearA.Y, M13 = linearA.Z }; linearJacobianB = new Matrix3f { M11 = -linearA.X, M12 = -linearA.Y, M13 = -linearA.Z }; angularJacobianA = new Matrix3f { M11 = angularA.X, M12 = angularA.Y, M13 = angularA.Z }; angularJacobianB = new Matrix3f { M11 = angularB.X, M12 = angularB.Y, M13 = angularB.Z }; }
/// <summary> /// Casts a convex shape against the collidable. /// </summary> /// <param name="castShape">Shape to cast.</param> /// <param name="startingTransform">Initial transform of the shape.</param> /// <param name="sweep">Sweep to apply to the shape.</param> /// <param name="hit">Hit data, if any.</param> /// <returns>Whether or not the cast hit anything.</returns> public override bool ConvexCast(CollisionShapes.ConvexShapes.ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3f sweep, out RayHit hit) { hit = new RayHit(); BoundingBox boundingBox; castShape.GetSweptLocalBoundingBox(ref startingTransform, ref worldTransform, ref sweep, out boundingBox); var tri = PhysicsThreadResources.GetTriangle(); var hitElements = CommonResources.GetIntList(); if (this.Shape.TriangleMesh.Tree.GetOverlaps(boundingBox, hitElements)) { hit.T = float.MaxValue; for (int i = 0; i < hitElements.Count; i++) { Shape.TriangleMesh.Data.GetTriangle(hitElements[i], out tri.vA, out tri.vB, out tri.vC); AffineTransform.Transform(ref tri.vA, ref worldTransform, out tri.vA); AffineTransform.Transform(ref tri.vB, ref worldTransform, out tri.vB); AffineTransform.Transform(ref tri.vC, ref worldTransform, out tri.vC); Vector3f center; Vector3f.Add(ref tri.vA, ref tri.vB, out center); Vector3f.Add(ref center, ref tri.vC, out center); Vector3f.Multiply(ref center, 1f / 3f, out center); Vector3f.Subtract(ref tri.vA, ref center, out tri.vA); Vector3f.Subtract(ref tri.vB, ref center, out tri.vB); Vector3f.Subtract(ref tri.vC, ref center, out tri.vC); tri.MaximumRadius = tri.vA.LengthSquared; float radius = tri.vB.LengthSquared; if (tri.MaximumRadius < radius) { tri.MaximumRadius = radius; } radius = tri.vC.LengthSquared; if (tri.MaximumRadius < radius) { tri.MaximumRadius = radius; } tri.MaximumRadius = (float)Math.Sqrt(tri.MaximumRadius); tri.collisionMargin = 0; var triangleTransform = new RigidTransform { Orientation = Quaternion.Identity, Position = center }; RayHit tempHit; if (MPRToolbox.Sweep(castShape, tri, ref sweep, ref Toolbox.ZeroVector, ref startingTransform, ref triangleTransform, out tempHit) && tempHit.T < hit.T) { hit = tempHit; } } tri.MaximumRadius = 0; PhysicsThreadResources.GiveBack(tri); CommonResources.GiveBack(hitElements); return(hit.T != float.MaxValue); } PhysicsThreadResources.GiveBack(tri); CommonResources.GiveBack(hitElements); return(false); }
/// <summary> /// Calculates and applies corrective impulses. /// Called automatically by space. /// </summary> public override float SolveIteration() { #if !WINDOWS Vector3f lambda = new Vector3f(); #else Vector3f lambda; #endif //Velocity along the length. Vector3f cross; Vector3f aVel, bVel; Vector3f.Cross(ref connectionA.angularVelocity, ref worldOffsetA, out cross); Vector3f.Add(ref connectionA.linearVelocity, ref cross, out aVel); Vector3f.Cross(ref connectionB.angularVelocity, ref worldOffsetB, out cross); Vector3f.Add(ref connectionB.linearVelocity, ref cross, out bVel); lambda.X = aVel.X - bVel.X + biasVelocity.X - softness * accumulatedImpulse.X; lambda.Y = aVel.Y - bVel.Y + biasVelocity.Y - softness * accumulatedImpulse.Y; lambda.Z = aVel.Z - bVel.Z + biasVelocity.Z - softness * accumulatedImpulse.Z; //Turn the velocity into an impulse. Vector3f.Transform(ref lambda, ref massMatrix, out lambda); //Accumulate the impulse Vector3f.Add(ref accumulatedImpulse, ref lambda, out accumulatedImpulse); //Apply the impulse //Constraint.applyImpulse(myConnectionA, myConnectionB, ref rA, ref rB, ref impulse); #if !WINDOWS Vector3f linear = new Vector3f(); #else Vector3f linear; #endif if (connectionA.isDynamic) { linear.X = -lambda.X; linear.Y = -lambda.Y; linear.Z = -lambda.Z; connectionA.ApplyLinearImpulse(ref linear); Vector3f taImpulse; Vector3f.Cross(ref worldOffsetA, ref linear, out taImpulse); connectionA.ApplyAngularImpulse(ref taImpulse); } if (connectionB.isDynamic) { connectionB.ApplyLinearImpulse(ref lambda); Vector3f tbImpulse; Vector3f.Cross(ref worldOffsetB, ref lambda, out tbImpulse); connectionB.ApplyAngularImpulse(ref tbImpulse); } return(Math.Abs(lambda.X) + Math.Abs(lambda.Y) + Math.Abs(lambda.Z)); }
public override void UpdateCollision(float dt) { WasContaining = Containing; WasTouching = Touching; var transform = new RigidTransform { Orientation = Quaternion.Identity }; DetectorVolume.TriangleMesh.Tree.GetOverlaps(convex.boundingBox, overlaps); for (int i = 0; i < overlaps.Count; i++) { DetectorVolume.TriangleMesh.Data.GetTriangle(overlaps.Elements[i], out triangle.vA, out triangle.vB, out triangle.vC); Vector3f.Add(ref triangle.vA, ref triangle.vB, out transform.Position); Vector3f.Add(ref triangle.vC, ref transform.Position, out transform.Position); Vector3f.Multiply(ref transform.Position, 1 / 3f, out transform.Position); Vector3f.Subtract(ref triangle.vA, ref transform.Position, out triangle.vA); Vector3f.Subtract(ref triangle.vB, ref transform.Position, out triangle.vB); Vector3f.Subtract(ref triangle.vC, ref transform.Position, out triangle.vC); //If this triangle collides with the convex, we can stop immediately since we know we're touching and not containing.))) //[MPR is used here in lieu of GJK because the MPR implementation tends to finish quicker when objects are overlapping than GJK. The GJK implementation does better on separated objects.] if (MPRToolbox.AreShapesOverlapping(convex.Shape, triangle, ref convex.worldTransform, ref transform)) { Touching = true; //The convex can't be fully contained if it's still touching the surface. Containing = false; overlaps.Clear(); goto events; } } overlaps.Clear(); //If we get here, then there was no shell intersection. //If the convex's center point is contained by the mesh, then the convex is fully contained. //If this is a child pair, the CheckContainment flag may be set to false. This is because the parent has //already determined that it is not contained (another child performed the check and found that it was not contained) //and that it is already touching somehow (either by intersection or by containment). //so further containment tests are unnecessary. if (CheckContainment && DetectorVolume.IsPointContained(ref convex.worldTransform.Position, overlaps)) { Touching = true; Containing = true; goto events; } //If we get here, then there was no surface intersection and the convex's center is not contained- the volume and convex are separate! Touching = false; Containing = false; events: NotifyDetectorVolumeOfChanges(); }
/// <summary> /// Multiplies a transform by another transform. /// </summary> /// <param name="a">First transform.</param> /// <param name="b">Second transform.</param> /// <param name="transform">Combined transform.</param> public static void Multiply(ref AffineTransform a, ref AffineTransform b, out AffineTransform transform) { Matrix3f linearTransform;//Have to use temporary variable just in case a or b reference is transform. Matrix3f.Multiply(ref a.LinearTransform, ref b.LinearTransform, out linearTransform); Vector3f translation; Vector3f.Transform(ref a.Translation, ref b.LinearTransform, out translation); Vector3f.Add(ref translation, ref b.Translation, out transform.Translation); transform.LinearTransform = linearTransform; }
///<summary> /// Gets the extreme point of the shape in world space in a given direction. ///</summary> ///<param name="direction">Direction to find the extreme point in.</param> /// <param name="shapeTransform">Transform to use for the shape.</param> ///<param name="extremePoint">Extreme point on the shape.</param> public void GetExtremePointWithoutMargin(Vector3f direction, ref RigidTransform shapeTransform, out Vector3f extremePoint) { Quaternion conjugate; Quaternion.Conjugate(ref shapeTransform.Orientation, out conjugate); Vector3f.Transform(ref direction, ref conjugate, out direction); GetLocalExtremePointWithoutMargin(ref direction, out extremePoint); Vector3f.Transform(ref extremePoint, ref shapeTransform.Orientation, out extremePoint); Vector3f.Add(ref extremePoint, ref shapeTransform.Position, out extremePoint); }
///<summary> /// Computes the bounding box of the transformed mesh shape. ///</summary> ///<param name="shapeTransform">Transform to apply to the shape during the bounding box calculation.</param> ///<param name="boundingBox">Bounding box containing the transformed mesh shape.</param> public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox) { //TODO: Could use an approximate bounding volume. Would be cheaper at runtime and use less memory, though the box would be bigger. Matrix3f o; Matrix3f.FromQuaternion(ref shapeTransform.Orientation, out o); GetBoundingBox(ref o, out boundingBox); Vector3f.Add(ref boundingBox.Max, ref shapeTransform.Position, out boundingBox.Max); Vector3f.Add(ref boundingBox.Min, ref shapeTransform.Position, out boundingBox.Min); }
void AddLocalContact(ref ContactData contact, ref Matrix3f orientation, ref QuickList <ContactData> candidatesToAdd) { //Put the contact into world space. Vector3f.Transform(ref contact.Position, ref orientation, out contact.Position); Vector3f.Add(ref contact.Position, ref convex.worldTransform.Position, out contact.Position); Vector3f.Transform(ref contact.Normal, ref orientation, out contact.Normal); //Check to see if the contact is unique before proceeding. if (IsContactUnique(ref contact, ref candidatesToAdd)) { candidatesToAdd.Add(ref contact); } }
///<summary> /// Multiplies a rigid transform by an affine transform. ///</summary> ///<param name="a">Rigid transform.</param> ///<param name="b">Affine transform.</param> ///<param name="transform">Combined transform.</param> public static void Multiply(ref RigidTransform a, ref AffineTransform b, out AffineTransform transform) { Matrix3f linearTransform;//Have to use temporary variable just in case b reference is transform. Matrix3f.FromQuaternion(ref a.Orientation, out linearTransform); Matrix3f.Multiply(ref linearTransform, ref b.LinearTransform, out linearTransform); Vector3f translation; Vector3f.Transform(ref a.Position, ref b.LinearTransform, out translation); Vector3f.Add(ref translation, ref b.Translation, out transform.Translation); transform.LinearTransform = linearTransform; }
/// <summary> /// Refreshes the contact manifold, removing any out of date contacts /// and updating others. /// </summary> public static void ContactRefresh(RawList <Contact> contacts, RawValueList <ContactSupplementData> supplementData, ref RigidTransform transformA, ref RigidTransform transformB, RawList <int> toRemove) { //TODO: Could also refresh normals with some trickery. //Would also need to refresh depth using new normals, and would require some extra information. for (int k = 0; k < contacts.Count; k++) { contacts.Elements[k].Validate(); ContactSupplementData data = supplementData.Elements[k]; Vector3f newPosA, newPosB; RigidTransform.Transform(ref data.LocalOffsetA, ref transformA, out newPosA); RigidTransform.Transform(ref data.LocalOffsetB, ref transformB, out newPosB); //ab - (ab*n)*n //Compute the horizontal offset. Vector3f ab; Vector3f.Subtract(ref newPosB, ref newPosA, out ab); float dot; Vector3f.Dot(ref ab, ref contacts.Elements[k].Normal, out dot); Vector3f temp; Vector3f.Multiply(ref contacts.Elements[k].Normal, dot, out temp); Vector3f.Subtract(ref ab, ref temp, out temp); dot = temp.LengthSquared; if (dot > CollisionDetectionSettings.ContactInvalidationLengthSquared) { toRemove.Add(k); } else { //Depth refresh: //Find deviation ((Ra-Rb)*N) and add to base depth. Vector3f.Dot(ref ab, ref contacts.Elements[k].Normal, out dot); contacts.Elements[k].PenetrationDepth = data.BasePenetrationDepth - dot; if (contacts.Elements[k].PenetrationDepth < -CollisionDetectionSettings.maximumContactDistance) { toRemove.Add(k); } else { //Refresh position and ra/rb. Vector3f newPos; Vector3f.Add(ref newPosB, ref newPosA, out newPos); Vector3f.Multiply(ref newPos, .5f, out newPos); contacts.Elements[k].Position = newPos; //This is an interesting idea, but has very little effect one way or the other. //data.BasePenetrationDepth = contacts.Elements[k].PenetrationDepth; //RigidTransform.TransformByInverse(ref newPos, ref transformA, out data.LocalOffsetA); //RigidTransform.TransformByInverse(ref newPos, ref transformB, out data.LocalOffsetB); } contacts.Elements[k].Validate(); } } }
///<summary> /// Gets the extreme point of the shape in world space in a given direction with margin expansion. ///</summary> ///<param name="direction">Direction to find the extreme point in.</param> /// <param name="shapeTransform">Transform to use for the shape.</param> ///<param name="extremePoint">Extreme point on the shape.</param> public void GetExtremePoint(Vector3f direction, ref RigidTransform shapeTransform, out Vector3f extremePoint) { GetExtremePointWithoutMargin(direction, ref shapeTransform, out extremePoint); float directionLength = direction.LengthSquared; if (directionLength > MathHelper.Epsilon) { Vector3f.Multiply(ref direction, collisionMargin / (float)Math.Sqrt(directionLength), out direction); Vector3f.Add(ref extremePoint, ref direction, out extremePoint); } }
protected internal override void GetContactInformation(int index, out ContactInformation info) { info.Contact = ContactManifold.contacts.Elements[index]; //Find the contact's normal force. float totalNormalImpulse = 0; info.NormalImpulse = 0; for (int i = 0; i < contactConstraint.penetrationConstraints.Count; i++) { totalNormalImpulse += contactConstraint.penetrationConstraints.Elements[i].accumulatedImpulse; if (contactConstraint.penetrationConstraints.Elements[i].contact == info.Contact) { info.NormalImpulse = contactConstraint.penetrationConstraints.Elements[i].accumulatedImpulse; } } //Compute friction force. Since we are using central friction, this is 'faked.' float radius; Vector3f.Distance(ref contactConstraint.slidingFriction.manifoldCenter, ref info.Contact.Position, out radius); if (totalNormalImpulse > 0) { info.FrictionImpulse = (info.NormalImpulse / totalNormalImpulse) * (contactConstraint.slidingFriction.accumulatedImpulse.Length + contactConstraint.twistFriction.accumulatedImpulse * radius); } else { info.FrictionImpulse = 0; } //Compute relative velocity Vector3f velocity; //If the pair is handling some type of query and does not actually have supporting entities, then consider the velocity contribution to be zero. if (EntityA != null) { Vector3f.Subtract(ref info.Contact.Position, ref EntityA.position, out velocity); Vector3f.Cross(ref EntityA.angularVelocity, ref velocity, out velocity); Vector3f.Add(ref velocity, ref EntityA.linearVelocity, out info.RelativeVelocity); } else { info.RelativeVelocity = new Vector3f(); } if (EntityB != null) { Vector3f.Subtract(ref info.Contact.Position, ref EntityB.position, out velocity); Vector3f.Cross(ref EntityB.angularVelocity, ref velocity, out velocity); Vector3f.Add(ref velocity, ref EntityB.linearVelocity, out velocity); Vector3f.Subtract(ref info.RelativeVelocity, ref velocity, out info.RelativeVelocity); } info.Pair = this; }
protected override void ComputeTangents() { if (ControlPoints.Count == 1) { tangents.Add(Vector3f.Zero); return; } if (ControlPoints.Count == 2) { Vector3f tangent = ControlPoints[1].Value - ControlPoints[0].Value; tangents.Add(tangent); tangents.Add(tangent); return; } Vector3f tangentA, tangentB; Vector3f previous, current, next; //First endpoint current = ControlPoints[0].Value; next = ControlPoints[1].Value; Vector3f.Subtract(ref next, ref current, out tangentA); Vector3f.Multiply(ref tangentA, (float)(.5 / (ControlPoints[1].Time - ControlPoints[0].Time)), out tangentA); //Vector3f.Multiply(ref current, .5f / (controlPoints[0].time), out tangentB); //Vector3f.Add(ref tangentA, ref tangentB, out tangentA); tangents.Add(tangentA); for (int i = 1; i < ControlPoints.Count - 1; i++) { previous = current; current = next; next = ControlPoints[i + 1].Value; Vector3f.Subtract(ref next, ref current, out tangentA); Vector3f.Subtract(ref current, ref previous, out tangentB); Vector3f.Multiply(ref tangentA, (float)(.5 / (ControlPoints[i + 1].Time - ControlPoints[i].Time)), out tangentA); Vector3f.Multiply(ref tangentB, (float)(.5 / (ControlPoints[i].Time - ControlPoints[i - 1].Time)), out tangentB); Vector3f.Add(ref tangentA, ref tangentB, out tangentA); tangents.Add(tangentA); } previous = current; current = next; Vector3f.Negate(ref current, out tangentA); Vector3f.Subtract(ref current, ref previous, out tangentB); int currentIndex = ControlPoints.Count - 1; int previousIndex = currentIndex - 1; //Vector3f.Multiply(ref tangentA, .5f / (-controlPoints[currentIndex].time), out tangentA); Vector3f.Multiply(ref tangentB, (float)(.5 / (ControlPoints[currentIndex].Time - ControlPoints[previousIndex].Time)), out tangentB); //Vector3f.Add(ref tangentA, ref tangentB, out tangentA); tangents.Add(tangentB); }
void GetBuoyancyInformation(EntityCollidable collidable, out float submergedVolume, out Vector3f submergedCenter) { BoundingBox entityBoundingBox; RigidTransform localTransform; RigidTransform.MultiplyByInverse(ref collidable.worldTransform, ref surfaceTransform, out localTransform); collidable.Shape.GetBoundingBox(ref localTransform, out entityBoundingBox); if (entityBoundingBox.Min.Y > 0) { //Fish out of the water. Don't need to do raycast tests on objects not at the boundary. submergedVolume = 0; submergedCenter = collidable.worldTransform.Position; return; } if (entityBoundingBox.Max.Y < 0) { submergedVolume = collidable.entity.CollisionInformation.Shape.Volume; submergedCenter = collidable.worldTransform.Position; return; } Vector3f origin, xSpacing, zSpacing; float perColumnArea; GetSamplingOrigin(ref entityBoundingBox, out xSpacing, out zSpacing, out perColumnArea, out origin); float boundingBoxHeight = entityBoundingBox.Max.Y - entityBoundingBox.Min.Y; float maxLength = -entityBoundingBox.Min.Y; submergedCenter = new Vector3f(); submergedVolume = 0; for (int i = 0; i < samplePointsPerDimension; i++) { for (int j = 0; j < samplePointsPerDimension; j++) { Vector3f columnVolumeCenter; float submergedHeight; if ((submergedHeight = GetSubmergedHeight(collidable, maxLength, boundingBoxHeight, ref origin, ref xSpacing, ref zSpacing, i, j, out columnVolumeCenter)) > 0) { float columnVolume = submergedHeight * perColumnArea; Vector3f.Multiply(ref columnVolumeCenter, columnVolume, out columnVolumeCenter); Vector3f.Add(ref columnVolumeCenter, ref submergedCenter, out submergedCenter); submergedVolume += columnVolume; } } } Vector3f.Divide(ref submergedCenter, submergedVolume, out submergedCenter); //Pull the submerged center into world space before applying the force. RigidTransform.Transform(ref submergedCenter, ref surfaceTransform, out submergedCenter); }
///<summary> /// Constructs a new kinematic capsule. ///</summary> ///<param name="start">Line segment start point.</param> ///<param name="end">Line segment end point.</param> ///<param name="radius">Radius of the capsule to expand the line segment by.</param> public Capsule(Vector3f start, Vector3f end, float radius) : this((end - start).Length, radius) { float length; Quaternion orientation; GetCapsuleInformation(ref start, ref end, out orientation, out length); this.Orientation = orientation; Vector3f position; Vector3f.Add(ref start, ref end, out position); Vector3f.Multiply(ref position, .5f, out position); this.Position = position; }
/// <summary> /// Cylinder shape used to compute the expanded bounding box of the character. /// </summary> void ExpandBoundingBox() { if (Body.ActivityInformation.IsActive) { //This runs after the bounding box updater is run, but before the broad phase. //Expanding the character's bounding box ensures that minor variations in velocity will not cause //any missed information. //TODO: seems a bit silly to do this work sequentially. Would be better if it could run in parallel in the proper location. var down = Down; var boundingBox = Body.CollisionInformation.BoundingBox; //Expand the bounding box up and down using the step height. Vector3f expansion; Vector3f.Multiply(ref down, StepManager.MaximumStepHeight, out expansion); expansion.X = Math.Abs(expansion.X); expansion.Y = Math.Abs(expansion.Y); expansion.Z = Math.Abs(expansion.Z); //When the character climbs a step, it teleports horizontally a little to gain support. Expand the bounding box to accommodate the margin. //Compute the expansion caused by the extra radius along each axis. //There's a few ways to go about doing this. //The following is heavily cooked, but it is based on the angle between the vertical axis and a particular axis. //Given that, the amount of the radial expansion required along that axis can be computed. //The dot product would provide the cos(angle) between the vertical axis and a chosen axis. //Equivalently, it is how much expansion would be along that axis, if the vertical axis was the axis of expansion. //However, it's not. The dot product actually gives us the expansion along an axis perpendicular to the chosen axis, pointing away from the character's vertical axis. //What we need is actually given by the sin(angle), which is given by ||verticalAxis x testAxis||. //The sin(angle) is the projected length of the verticalAxis (not the expansion!) on the axis perpendicular to the testAxis pointing away from the character's vertical axis. //That projected length, however is equal to the expansion along the test axis, which is exactly what we want. //To show this, try setting up the triangles at the corner of a cylinder with the world axes and cylinder axes. //Since the test axes we're using are all standard directions ({0,0,1}, {0,1,0}, and {0,0,1}), most of the cross product logic simplifies out, and we are left with: var horizontalExpansionAmount = Body.CollisionInformation.Shape.CollisionMargin * 1.1f; Vector3f squaredDown; squaredDown.X = down.X * down.X; squaredDown.Y = down.Y * down.Y; squaredDown.Z = down.Z * down.Z; expansion.X += horizontalExpansionAmount * (float)Math.Sqrt(squaredDown.Y + squaredDown.Z); expansion.Y += horizontalExpansionAmount * (float)Math.Sqrt(squaredDown.X + squaredDown.Z); expansion.Z += horizontalExpansionAmount * (float)Math.Sqrt(squaredDown.X + squaredDown.Y); Vector3f.Add(ref expansion, ref boundingBox.Max, out boundingBox.Max); Vector3f.Subtract(ref boundingBox.Min, ref expansion, out boundingBox.Min); Body.CollisionInformation.BoundingBox = boundingBox; } }
///<summary> /// Gets the extreme point of the minkowski difference of shapeA and shapeB in the local space of shapeA. ///</summary> ///<param name="shapeA">First shape.</param> ///<param name="shapeB">Second shape.</param> ///<param name="direction">Extreme point direction in local space.</param> ///<param name="localTransformB">Transform of shapeB in the local space of A.</param> ///<param name="extremePoint">The extreme point in the local space of A.</param> public static void GetLocalMinkowskiExtremePoint(ConvexShape shapeA, ConvexShape shapeB, ref Vector3f direction, ref RigidTransform localTransformB, out Vector3f extremePoint) { //Extreme point of A-B along D = (extreme point of A along D) - (extreme point of B along -D) shapeA.GetLocalExtremePointWithoutMargin(ref direction, out extremePoint); Vector3f v; Vector3f negativeN; Vector3f.Negate(ref direction, out negativeN); shapeB.GetExtremePointWithoutMargin(negativeN, ref localTransformB, out v); Vector3f.Subtract(ref extremePoint, ref v, out extremePoint); ExpandMinkowskiSum(shapeA.collisionMargin, shapeB.collisionMargin, ref direction, out v); Vector3f.Add(ref extremePoint, ref v, out extremePoint); }
public void GetEntries(BoundingSphere boundingShape, IList <BroadPhaseEntry> overlaps) { //Create a bounding box based on the bounding sphere. //Compute the min and max of the bounding box. //Loop through the cells and select bounding boxes which overlap the x axis. #if !WINDOWS Vector3f offset = new Vector3f(); #else Vector3f offset; #endif offset.X = boundingShape.Radius; offset.Y = offset.X; offset.Z = offset.Y; BoundingBox box; Vector3f.Add(ref boundingShape.Center, ref offset, out box.Max); Vector3f.Subtract(ref boundingShape.Center, ref offset, out box.Min); Int2 min, max; Grid2DSortAndSweep.ComputeCell(ref box.Min, out min); Grid2DSortAndSweep.ComputeCell(ref box.Max, out max); for (int i = min.Y; i <= max.Y; i++) { for (int j = min.Z; j <= max.Z; j++) { //Grab the cell that we are currently in. Int2 cellIndex; cellIndex.Y = i; cellIndex.Z = j; GridCell2D cell; if (owner.cellSet.TryGetCell(ref cellIndex, out cell)) { //To fully accelerate this, the entries list would need to contain both min and max interval markers. //Since it only contains the sorted min intervals, we can't just start at a point in the middle of the list. //Consider some giant bounding box that spans the entire list. for (int k = 0; k < cell.entries.Count && cell.entries.Elements[k].item.boundingBox.Min.X <= box.Max.X; k++) //TODO: Try additional x axis pruning? A bit of optimization potential due to overlap with AABB test. { bool intersects; var item = cell.entries.Elements[k].item; item.boundingBox.Intersects(ref boundingShape, out intersects); if (intersects && !overlaps.Contains(item)) { overlaps.Add(item); } } } } } }
/// <summary> /// Calculates and applies corrective impulses. /// Called automatically by space. /// </summary> public override float SolveIteration() { float linearSpeed = entity.linearVelocity.LengthSquared; if (linearSpeed > maximumSpeedSquared) { linearSpeed = (float)Math.Sqrt(linearSpeed); Vector3f impulse; //divide by linearSpeed to normalize the velocity. //Multiply by linearSpeed - maximumSpeed to get the 'velocity change vector.' Vector3f.Multiply(ref entity.linearVelocity, -(linearSpeed - maximumSpeed) / linearSpeed, out impulse); //incorporate softness Vector3f softnessImpulse; Vector3f.Multiply(ref accumulatedImpulse, usedSoftness, out softnessImpulse); Vector3f.Subtract(ref impulse, ref softnessImpulse, out impulse); //Transform into impulse Vector3f.Multiply(ref impulse, effectiveMassMatrix, out impulse); //Accumulate Vector3f previousAccumulatedImpulse = accumulatedImpulse; Vector3f.Add(ref accumulatedImpulse, ref impulse, out accumulatedImpulse); float forceMagnitude = accumulatedImpulse.LengthSquared; if (forceMagnitude > maxForceDtSquared) { //max / impulse gives some value 0 < x < 1. Basically, normalize the vector (divide by the length) and scale by the maximum. float multiplier = maxForceDt / (float)Math.Sqrt(forceMagnitude); accumulatedImpulse.X *= multiplier; accumulatedImpulse.Y *= multiplier; accumulatedImpulse.Z *= multiplier; //Since the limit was exceeded by this corrective impulse, limit it so that the accumulated impulse remains constrained. impulse.X = accumulatedImpulse.X - previousAccumulatedImpulse.X; impulse.Y = accumulatedImpulse.Y - previousAccumulatedImpulse.Y; impulse.Z = accumulatedImpulse.Z - previousAccumulatedImpulse.Z; } entity.ApplyLinearImpulse(ref impulse); return(Math.Abs(impulse.X) + Math.Abs(impulse.Y) + Math.Abs(impulse.Z)); } return(0); }
///<summary> /// Gets the extreme point of the shape in local space in a given direction. ///</summary> ///<param name="direction">Direction to find the extreme point in.</param> ///<param name="extremePoint">Extreme point on the shape.</param> public override void GetLocalExtremePointWithoutMargin(ref Vector3f direction, out Vector3f extremePoint) { var transform = new RigidTransform { Orientation = shapes.WrappedList.Elements[0].Orientation }; shapes.WrappedList.Elements[0].CollisionShape.GetExtremePoint(direction, ref transform, out extremePoint); for (int i = 1; i < shapes.WrappedList.Count; i++) { Vector3f temp; transform.Orientation = shapes.WrappedList.Elements[i].Orientation; shapes.WrappedList.Elements[i].CollisionShape.GetExtremePoint(direction, ref transform, out temp); Vector3f.Add(ref extremePoint, ref temp, out extremePoint); } Vector3f.Add(ref extremePoint, ref localOffset, out extremePoint); }
protected internal override void UpdateJacobiansAndVelocityBias() { //Transform the anchors and offsets into world space. Vector3f offsetA, offsetB, lineDirection; Vector3f.Transform(ref LocalPlaneAnchor, ref ConnectionA.Orientation, out offsetA); Vector3f.Transform(ref LocalPlaneNormal, ref ConnectionA.Orientation, out lineDirection); Vector3f.Transform(ref LocalAnchorB, ref ConnectionB.Orientation, out offsetB); Vector3f anchorA, anchorB; Vector3f.Add(ref ConnectionA.Position, ref offsetA, out anchorA); Vector3f.Add(ref ConnectionB.Position, ref offsetB, out anchorB); //Compute the distance. Vector3f separation; Vector3f.Subtract(ref anchorB, ref anchorA, out separation); //This entire constraint is very similar to the IKDistanceLimit, except the current distance is along an axis. float currentDistance; Vector3f.Dot(ref separation, ref lineDirection, out currentDistance); velocityBias = new Vector3f(errorCorrectionFactor * currentDistance, 0, 0); //Compute jacobians Vector3f angularA, angularB; //We can't just use the offset to anchor for A's jacobian- the 'collision' location is way out there at anchorB! Vector3f rA; Vector3f.Subtract(ref anchorB, ref ConnectionA.Position, out rA); Vector3f.Cross(ref rA, ref lineDirection, out angularA); //linearB = -linearA, so just swap the cross product order. Vector3f.Cross(ref lineDirection, ref offsetB, out angularB); //Put all the 1x3 jacobians into a 3x3 matrix representation. linearJacobianA = new Matrix3f { M11 = lineDirection.X, M12 = lineDirection.Y, M13 = lineDirection.Z }; linearJacobianB = new Matrix3f { M11 = -lineDirection.X, M12 = -lineDirection.Y, M13 = -lineDirection.Z }; angularJacobianA = new Matrix3f { M11 = angularA.X, M12 = angularA.Y, M13 = angularA.Z }; angularJacobianB = new Matrix3f { M11 = angularB.X, M12 = angularB.Y, M13 = angularB.Z }; }
/// <summary> /// Computes one iteration of the constraint to meet the solver updateable's goal. /// </summary> /// <returns>The rough applied impulse magnitude.</returns> public override float SolveIteration() { //Compute relative velocity Vector3f lambda; Vector3f.Cross(ref r, ref entity.angularVelocity, out lambda); Vector3f.Subtract(ref lambda, ref entity.linearVelocity, out lambda); //Add in bias velocity Vector3f.Add(ref biasVelocity, ref lambda, out lambda); //Add in softness Vector3f softnessVelocity; Vector3f.Multiply(ref accumulatedImpulse, usedSoftness, out softnessVelocity); Vector3f.Subtract(ref lambda, ref softnessVelocity, out lambda); //In terms of an impulse (an instantaneous change in momentum), what is it? Vector3f.Transform(ref lambda, ref effectiveMassMatrix, out lambda); //Sum the impulse. Vector3f previousAccumulatedImpulse = accumulatedImpulse; accumulatedImpulse += lambda; //If the impulse it takes to get to the goal is too high for the motor to handle, scale it back. float sumImpulseLengthSquared = accumulatedImpulse.LengthSquared; if (sumImpulseLengthSquared > maxForceDtSquared) { //max / impulse gives some value 0 < x < 1. Basically, normalize the vector (divide by the length) and scale by the maximum. accumulatedImpulse *= maxForceDt / (float)Math.Sqrt(sumImpulseLengthSquared); //Since the limit was exceeded by this corrective impulse, limit it so that the accumulated impulse remains constrained. lambda = accumulatedImpulse - previousAccumulatedImpulse; } entity.ApplyLinearImpulse(ref lambda); Vector3f taImpulse; Vector3f.Cross(ref r, ref lambda, out taImpulse); entity.ApplyAngularImpulse(ref taImpulse); return(Math.Abs(lambda.X) + Math.Abs(lambda.Y) + Math.Abs(lambda.Z)); }
///<summary> /// Computes the expansion of the minkowski sum due to margins in a given direction. ///</summary> ///<param name="marginA">First margin.</param> ///<param name="marginB">Second margin.</param> ///<param name="direction">Extreme point direction.</param> ///<param name="toExpandA">Margin contribution to the shapeA.</param> ///<param name="toExpandB">Margin contribution to the shapeB.</param> public static void ExpandMinkowskiSum(float marginA, float marginB, Vector3f direction, ref Vector3f toExpandA, ref Vector3f toExpandB) { float lengthSquared = direction.LengthSquared; if (lengthSquared > MathHelper.Epsilon) { lengthSquared = 1 / (float)Math.Sqrt(lengthSquared); //The contribution to the minkowski sum by the margin is: //direction * marginA - (-direction) * marginB. Vector3f contribution; Vector3f.Multiply(ref direction, marginA * lengthSquared, out contribution); Vector3f.Add(ref toExpandA, ref contribution, out toExpandA); Vector3f.Multiply(ref direction, marginB * lengthSquared, out contribution); Vector3f.Subtract(ref toExpandB, ref contribution, out toExpandB); } //If the direction is too small, then the expansion values are left unchanged. }
/// <summary> /// Computes one iteration of the constraint to meet the solver updateable's goal. /// </summary> /// <returns>The rough applied impulse magnitude.</returns> public override float SolveIteration() { //TODO: This could technically be faster. //Form the jacobian explicitly. //Cross cross add add subtract dot //vs //dot dot dot dot and then scalar adds Vector3f dv; Vector3f aVel, bVel; Vector3f.Cross(ref connectionA.angularVelocity, ref rA, out aVel); Vector3f.Add(ref aVel, ref connectionA.linearVelocity, out aVel); Vector3f.Cross(ref connectionB.angularVelocity, ref rB, out bVel); Vector3f.Add(ref bVel, ref connectionB.linearVelocity, out bVel); Vector3f.Subtract(ref aVel, ref bVel, out dv); float velocityDifference; Vector3f.Dot(ref dv, ref worldPlaneNormal, out velocityDifference); //if(velocityDifference > 0) // Debug.WriteLine("Velocity difference: " + velocityDifference); //Debug.WriteLine("softness velocity: " + softness * accumulatedImpulse); float lambda = negativeEffectiveMass * (velocityDifference + biasVelocity + softness * accumulatedImpulse); accumulatedImpulse += lambda; Vector3f impulse; Vector3f torque; Vector3f.Multiply(ref worldPlaneNormal, lambda, out impulse); if (connectionA.isDynamic) { Vector3f.Multiply(ref rAcrossN, lambda, out torque); connectionA.ApplyLinearImpulse(ref impulse); connectionA.ApplyAngularImpulse(ref torque); } if (connectionB.isDynamic) { Vector3f.Negate(ref impulse, out impulse); Vector3f.Multiply(ref rBcrossN, lambda, out torque); connectionB.ApplyLinearImpulse(ref impulse); connectionB.ApplyAngularImpulse(ref torque); } return(lambda); }
///<summary> /// Gets the point on the segment closest to the origin. ///</summary> ///<param name="point">Point closest to origin.</param> public void GetPointOnSegmentClosestToOrigin(out Vector3f point) { Vector3f segmentDisplacement; Vector3f.Subtract(ref B, ref A, out segmentDisplacement); float dotA; Vector3f.Dot(ref segmentDisplacement, ref A, out dotA); if (dotA > 0) { //'Behind' segment. This can't happen in a boolean version, //but with closest points warmstarting or raycasts, it will. State = SimplexState.Point; U = 1; point = A; return; } float dotB; Vector3f.Dot(ref segmentDisplacement, ref B, out dotB); if (dotB > 0) { //Inside segment. U = dotB / segmentDisplacement.LengthSquared; V = 1 - U; Vector3f.Multiply(ref segmentDisplacement, V, out point); Vector3f.Add(ref point, ref A, out point); return; } //It should be possible in the warmstarted closest point calculation/raycasting to be outside B. //It is not possible in a 'boolean' GJK, where it early outs as soon as a separating axis is found. //Outside B. //Remove current A; we're becoming a point. A = B; SimplexA.A = SimplexA.B; SimplexB.A = SimplexB.B; State = SimplexState.Point; U = 1; point = A; }
public void Raycast(Vector3f origin, Vector3f direction, out Vector3f hitPoint) { Jitter.LinearMath.JVector outNormal; RigidBody outBody; float outFraction; bool hit = world.CollisionSystem.Raycast(new Jitter.LinearMath.JVector(origin.x, origin.y, origin.z), new Jitter.LinearMath.JVector(direction.x, direction.y, direction.z), null, out outBody, out outNormal, out outFraction); if (hit) { hitPoint = origin.Add(direction.Multiply(outFraction)); } else { hitPoint = null; } }
internal bool IsPointContained(ref Vector3f point, RawList <int> triangles) { Vector3f rayDirection; //Point from the approximate center of the mesh outwards. //This is a cheap way to reduce the number of unnecessary checks when objects are external to the mesh. Vector3f.Add(ref boundingBox.Max, ref boundingBox.Min, out rayDirection); Vector3f.Multiply(ref rayDirection, .5f, out rayDirection); Vector3f.Subtract(ref point, ref rayDirection, out rayDirection); //If the point is right in the middle, we'll need a backup. if (rayDirection.LengthSquared < .01f) { rayDirection = VectorHelper.Up; } var ray = new Ray(point, rayDirection); triangleMesh.Tree.GetOverlaps(ray, triangles); float minimumT = float.MaxValue; bool minimumIsClockwise = false; for (int i = 0; i < triangles.Count; i++) { Vector3f a, b, c; triangleMesh.Data.GetTriangle(triangles.Elements[i], out a, out b, out c); RayHit hit; bool hitClockwise; if (Toolbox.FindRayTriangleIntersection(ref ray, float.MaxValue, ref a, ref b, ref c, out hitClockwise, out hit)) { if (hit.T < minimumT) { minimumT = hit.T; minimumIsClockwise = hitClockwise; } } } triangles.Clear(); //If the first hit is on the inner surface, then the ray started inside the mesh. return(minimumT < float.MaxValue && minimumIsClockwise == innerFacingIsClockwise); }
public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox) { #if !WINDOWS boundingBox = new BoundingBox(); #endif Vector3f upExtreme; Vector3f.TransformY(halfLength, ref shapeTransform.Orientation, out upExtreme); if (upExtreme.X > 0) { boundingBox.Max.X = upExtreme.X + collisionMargin; boundingBox.Min.X = -upExtreme.X - collisionMargin; } else { boundingBox.Max.X = -upExtreme.X + collisionMargin; boundingBox.Min.X = upExtreme.X - collisionMargin; } if (upExtreme.Y > 0) { boundingBox.Max.Y = upExtreme.Y + collisionMargin; boundingBox.Min.Y = -upExtreme.Y - collisionMargin; } else { boundingBox.Max.Y = -upExtreme.Y + collisionMargin; boundingBox.Min.Y = upExtreme.Y - collisionMargin; } if (upExtreme.Z > 0) { boundingBox.Max.Z = upExtreme.Z + collisionMargin; boundingBox.Min.Z = -upExtreme.Z - collisionMargin; } else { boundingBox.Max.Z = -upExtreme.Z + collisionMargin; boundingBox.Min.Z = upExtreme.Z - collisionMargin; } Vector3f.Add(ref shapeTransform.Position, ref boundingBox.Min, out boundingBox.Min); Vector3f.Add(ref shapeTransform.Position, ref boundingBox.Max, out boundingBox.Max); }