/// <summary> /// Applies the sequential impulse. /// </summary> public override float SolveIteration() { float lambda; System.Numerics.Vector3 relativeVelocity; Vector3Ex.Subtract(ref connectionA.angularVelocity, ref connectionB.angularVelocity, out relativeVelocity); //Transform the velocity to with the jacobian Vector3Ex.Dot(ref relativeVelocity, ref hingeAxis, out lambda); //Add in the constraint space bias velocity lambda = -lambda + biasVelocity - softness * accumulatedImpulse; //Transform to an impulse lambda *= velocityToImpulse; //Clamp accumulated impulse (can't go negative) float previousAccumulatedImpulse = accumulatedImpulse; accumulatedImpulse = MathHelper.Max(accumulatedImpulse + lambda, 0); lambda = accumulatedImpulse - previousAccumulatedImpulse; //Apply the impulse System.Numerics.Vector3 impulse; Vector3Ex.Multiply(ref hingeAxis, lambda, out impulse); if (connectionA.isDynamic) { connectionA.ApplyAngularImpulse(ref impulse); } if (connectionB.isDynamic) { Vector3Ex.Negate(ref impulse, out impulse); connectionB.ApplyAngularImpulse(ref impulse); } return(Math.Abs(lambda)); }
public override (double u, double v) GetUv(IntersectInfo info) { Vector3 vn = new Vector3(0, 1, 0).GetNormal(); // north pole / up Vector3 ve = new Vector3(0, 0, 1).GetNormal(); // equator / sphere orientation Vector3 vp = (info.HitPosition - position).GetNormal(); //points from center of sphere to intersection double phi = Math.Acos(-Vector3Ex.Dot(vp, vn)); double v = (phi * 2 / Math.PI) - 1; double sinphi = Vector3Ex.Dot(ve, vp) / Math.Sin(phi); sinphi = sinphi < -1 ? -1 : sinphi > 1 ? 1 : sinphi; double theta = Math.Acos(sinphi) * 2 / Math.PI; double u; if (Vector3Ex.Dot(Vector3Ex.Cross(vn, ve), vp) > 0) { u = theta; } else { u = 1 - theta; } return(u, v); }
private Ray GetRefractionRay(Vector3 P, Vector3 N, Vector3 V, double refraction) { int method = 0; switch (method) { case 0: return(new Ray(P, V, Ray.sameSurfaceOffset, double.MaxValue)); case 1: V = V * -1; double n = -0.55; // refraction constant for now if (n < 0 || n > 1) { return(new Ray(P, V)); // no refraction } break; case 2: double c1 = Vector3Ex.Dot(N, V); double c2 = 1 - refraction * refraction * (1 - c1 * c1); if (c2 < 0) { c2 = Math.Sqrt(c2); } Vector3 T = (N * (refraction * c1 - c2) - V * refraction) * -1; T.Normalize(); return(new Ray(P, T)); // no refraction } return(new Ray(P, V, Ray.sameSurfaceOffset, double.MaxValue)); }
public override void OnMouseDown(Mouse3DEventArgs mouseEvent3D) { if (mouseEvent3D.info != null && Object3DControlContext.Scene.SelectedItem != null) { hadClickOnControl = true; activeSelectedItem = RootSelection; zValueDisplayInfo.Visible = true; var selectedItem = activeSelectedItem; double distanceToHit = Vector3Ex.Dot(mouseEvent3D.info.HitPosition, mouseEvent3D.MouseRay.directionNormal); hitPlane = new PlaneShape(mouseEvent3D.MouseRay.directionNormal, distanceToHit, null); originalPointToMove = GetTopPosition(selectedItem); initialHitPosition = mouseEvent3D.info.HitPosition; if (selectedItem is IObjectWithHeight heightObject) { heightOnMouseDown = heightObject.Height; } mouseDownSelectedBounds = selectedItem.GetAxisAlignedBoundingBox(); } base.OnMouseDown(mouseEvent3D); }
private Ray GetReflectionRay(Vector3 p, Vector3 n, Vector3 v) { double c1 = -Vector3Ex.Dot(n, v); Vector3 rl = v + (n * 2 * c1); return(new Ray(p, rl, Ray.sameSurfaceOffset, double.MaxValue)); }
/// <summary> /// Compute the point resulting from the intersection with a plane /// </summary> /// <param name="normal">the plane normal</param> /// <param name="planePoint">a plane point.</param> /// <returns>intersection point.If they don't intersect, return null</returns> public Vector3 ComputePlaneIntersection(Plane plane) { double distanceToStartFromOrigin = Vector3Ex.Dot(plane.Normal, startPoint); double distanceFromPlane = distanceToStartFromOrigin - plane.DistanceFromOrigin; double denominator = Vector3Ex.Dot(plane.Normal, Direction); if (Math.Abs(denominator) < EqualityTolerance) { //if line is parallel to the plane... if (Math.Abs(distanceFromPlane) < EqualityTolerance) { //if line is contained in the plane... return(startPoint); } else { return(Vector3.PositiveInfinity); } } else // line intercepts the plane... { double t = -distanceFromPlane / denominator; Vector3 resultPoint = new Vector3(); resultPoint.X = startPoint.X + t * Direction.X; resultPoint.Y = startPoint.Y + t * Direction.Y; resultPoint.Z = startPoint.Z + t * Direction.Z; return(resultPoint); } }
public TriangleShape(Vector3 vertex0, Vector3 vertex1, Vector3 vertex2, MaterialAbstract material) { Vector3 planeNormal = Vector3Ex.Cross(vertex1 - vertex0, vertex2 - vertex0).GetNormal(); double distanceFromOrigin = Vector3Ex.Dot(vertex0, planeNormal); Plane = new PlaneFloat(new Vector3Float(planeNormal), (float)distanceFromOrigin); Material = material; vertices[0] = new Vector3Float(vertex0); vertices[1] = new Vector3Float(vertex1); vertices[2] = new Vector3Float(vertex2); center = new Vector3Float((vertex0 + vertex1 + vertex2) / 3); var normalLengths = new [] { Math.Abs(planeNormal.X), Math.Abs(planeNormal.Y), Math.Abs(planeNormal.Z) }; MajorAxis = (byte)normalLengths.Select((v, i) => new { Axis = i, Value = Math.Abs(v) }).OrderBy(o => o.Value).Last().Axis; for (int i = 0; i < 3; i++) { boundsOnMajorAxis.Left = Math.Min(vertices[i][xForMajorAxis], boundsOnMajorAxis.Left); boundsOnMajorAxis.Right = Math.Max(vertices[i][xForMajorAxis], boundsOnMajorAxis.Right); boundsOnMajorAxis.Bottom = Math.Min(vertices[i][yForMajorAxis], boundsOnMajorAxis.Bottom); boundsOnMajorAxis.Top = Math.Max(vertices[i][yForMajorAxis], boundsOnMajorAxis.Top); } aabbMinXYZ = vertices[0].ComponentMin(vertices[1]).ComponentMin(vertices[2]); aabbMaxXYZ = vertices[0].ComponentMax(vertices[1]).ComponentMax(vertices[2]); }
/// <summary> /// Updates the movement basis of the horizontal motion constraint. /// Should be updated automatically by the character on each time step; other code should not need to call this. /// </summary> /// <param name="forward">Forward facing direction of the character.</param> public void UpdateMovementBasis(ref System.Numerics.Vector3 forward) { System.Numerics.Vector3 down = characterBody.orientationMatrix.Down; System.Numerics.Vector3 strafeDirection; System.Numerics.Vector3 horizontalForwardDirection = forward - down * Vector3Ex.Dot(down, forward); float forwardLengthSquared = horizontalForwardDirection.LengthSquared(); if (forwardLengthSquared < Toolbox.Epsilon) { //Use an arbitrary direction to complete the basis. horizontalForwardDirection = characterBody.orientationMatrix.Forward; strafeDirection = characterBody.orientationMatrix.Right; } else { Vector3Ex.Divide(ref horizontalForwardDirection, (float)Math.Sqrt(forwardLengthSquared), out horizontalForwardDirection); Vector3Ex.Cross(ref down, ref horizontalForwardDirection, out strafeDirection); //Don't need to normalize the strafe direction; it's the cross product of two normalized perpendicular vectors. } Vector3Ex.Multiply(ref horizontalForwardDirection, movementDirection.Y, out movementDirection3d); System.Numerics.Vector3 strafeComponent; Vector3Ex.Multiply(ref strafeDirection, movementDirection.X, out strafeComponent); Vector3Ex.Add(ref strafeComponent, ref movementDirection3d, out movementDirection3d); }
public void CalculateFrustum(int width, int height, Vector3 origin) { frustumForRays.Planes = new Plane[4]; Vector3 cornerRay0 = rayArray[0].directionNormal; Vector3 cornerRay1 = rayArray[width - 1].directionNormal; Vector3 cornerRay2 = rayArray[(height - 1) * width].directionNormal; Vector3 cornerRay3 = rayArray[(height - 1) * width + (width - 1)].directionNormal; { Vector3 normal = Vector3Ex.Cross(cornerRay0, cornerRay1).GetNormal(); frustumForRays.Planes[0] = new Plane(normal, Vector3Ex.Dot(normal, origin)); } { Vector3 normal = Vector3Ex.Cross(cornerRay1, cornerRay2).GetNormal(); frustumForRays.Planes[1] = new Plane(normal, Vector3Ex.Dot(normal, origin)); } { Vector3 normal = Vector3Ex.Cross(cornerRay2, cornerRay3).GetNormal(); frustumForRays.Planes[2] = new Plane(normal, Vector3Ex.Dot(normal, origin)); } { Vector3 normal = Vector3Ex.Cross(cornerRay3, cornerRay0).GetNormal(); frustumForRays.Planes[3] = new Plane(normal, Vector3Ex.Dot(normal, origin)); } }
/// <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() { float velocityA, velocityB; //Find the velocity contribution from each connection Vector3Ex.Dot(ref connectionA.angularVelocity, ref jacobianA, out velocityA); Vector3Ex.Dot(ref connectionB.angularVelocity, ref jacobianB, out velocityB); //Add in the constraint space bias velocity float lambda = (-velocityA - velocityB) - biasVelocity - softness * accumulatedImpulse; //Transform to an impulse lambda *= velocityToImpulse; //Clamp accumulated impulse (can't go negative) float previousAccumulatedImpulse = accumulatedImpulse; accumulatedImpulse = MathHelper.Min(accumulatedImpulse + lambda, 0); lambda = accumulatedImpulse - previousAccumulatedImpulse; //Apply the impulse System.Numerics.Vector3 impulse; if (connectionA.isDynamic) { Vector3Ex.Multiply(ref jacobianA, lambda, out impulse); connectionA.ApplyAngularImpulse(ref impulse); } if (connectionB.isDynamic) { Vector3Ex.Multiply(ref jacobianB, lambda, out impulse); connectionB.ApplyAngularImpulse(ref impulse); } return(Math.Abs(lambda)); }
private Ray GetRefractionRay(Vector3 p, Vector3 n, Vector3 v, double refraction) { int method = 0; switch (method) { case 0: return(new Ray(p, v, Ray.sameSurfaceOffset, double.MaxValue)); case 1: v *= -1; double newRefraction = -0.55; // refraction constant for now if (newRefraction < 0 || newRefraction > 1) { return(new Ray(p, v)); // no refraction } break; case 2: double c1 = Vector3Ex.Dot(n, v); double c2 = 1 - refraction * refraction * (1 - c1 * c1); if (c2 < 0) { c2 = Math.Sqrt(c2); } Vector3 t = (n * (refraction * c1 - c2) - v * refraction) * -1; t.Normalize(); return(new Ray(p, t)); // no refraction } return(new Ray(p, v, Ray.sameSurfaceOffset, double.MaxValue)); }
bool IsObstructiveToUpStepping(ref System.Numerics.Vector3 sideNormal, ref ContactData contact) { //Up stepping has slightly different rules than down stepping. //For contacts with normals aligned with the side normal that triggered the step, //only marginal (allowed penetration) obstruction is permitted. //Consider the side normal to define an implicit plane. float dot; Vector3Ex.Dot(ref contact.Normal, ref sideNormal, out dot); if (dot * contact.PenetrationDepth > CollisionDetectionSettings.AllowedPenetration) { //It's too deep! Can't step. return(true); } //Go through side-facing contact and check to see if the new contact is deeper than any existing contact in the direction of the existing contact. //This is equivalent to considering the existing contacts to define planes and then comparing the new contact against those planes. //Since we already have the penetration depths, we don't need to use the positions of the contacts. foreach (var c in SupportFinder.SideContacts) { dot = Vector3Ex.Dot(contact.Normal, c.Contact.Normal); float depth = dot * c.Contact.PenetrationDepth; if (depth > Math.Max(c.Contact.PenetrationDepth, CollisionDetectionSettings.AllowedPenetration)) { return(true); } } return(false); }
/// <summary> /// Performs any per-frame computation needed by the constraint. /// </summary> /// <param name="dt">Time step duration.</param> public override void Update(float dt) { //Collect references, pick the mode, and configure the coefficients to be used by the solver. if (supportData.SupportObject != null) { //Get an easy reference to the support. var support = supportData.SupportObject as EntityCollidable; if (support != null) { supportEntity = support.Entity; } else { supportEntity = null; } } else { supportEntity = null; } maximumForce = maximumGlueForce * dt; //If we don't allow the character to get out of the ground, it could apply some significant forces to a dynamic support object. //Technically, there exists a better estimate of the necessary speed, but choosing the maximum position correction speed is a nice catch-all. //If you change that correction speed, watch out!!! It could significantly change the way the character behaves when trying to glue to surfaces. if (supportData.Depth > 0) { permittedVelocity = CollisionResponseSettings.MaximumPositionCorrectionSpeed; } else { permittedVelocity = 0; } //Compute the jacobians and effective mass matrix. This constraint works along a single degree of freedom, so the mass matrix boils down to a scalar. linearJacobianA = supportData.Normal; Vector3.Negate(ref linearJacobianA, out linearJacobianB); effectiveMass = character.Body.InverseMass; if (supportEntity != null) { Vector3 offsetB = supportData.Position - supportEntity.Position; Vector3.Cross(ref offsetB, ref linearJacobianB, out angularJacobianB); if (supportEntity.IsDynamic) { //Only dynamic entities can actually contribute anything to the effective mass. //Kinematic entities have infinite mass and inertia, so this would all zero out. Matrix3X3 inertiaInverse = supportEntity.LocalInertiaTensorInverse; Vector3 angularComponentB; Matrix3X3.Transform(ref angularJacobianB, ref inertiaInverse, out angularComponentB); float effectiveMassContribution; Vector3Ex.Dot(ref angularComponentB, ref angularJacobianB, out effectiveMassContribution); effectiveMass += supportForceFactor * (effectiveMassContribution + supportEntity.InverseMass); } } effectiveMass = 1 / effectiveMass; //So much nicer and shorter than the horizontal constraint! }
/// <summary> /// Solves for velocity. /// </summary> public override float SolveIteration() { float velocityA, velocityB; //Find the velocity contribution from each connection Vector3Ex.Dot(ref connectionA.angularVelocity, ref jacobianA, out velocityA); Vector3Ex.Dot(ref connectionB.angularVelocity, ref jacobianB, out velocityB); //Add in the constraint space bias velocity float lambda = -(velocityA + velocityB) - biasVelocity - softness * accumulatedImpulse; //Transform to an impulse lambda *= velocityToImpulse; //Accumulate the impulse accumulatedImpulse += lambda; //Apply the impulse System.Numerics.Vector3 impulse; if (connectionA.isDynamic) { Vector3Ex.Multiply(ref jacobianA, lambda, out impulse); connectionA.ApplyAngularImpulse(ref impulse); } if (connectionB.isDynamic) { Vector3Ex.Multiply(ref jacobianB, lambda, out impulse); connectionB.ApplyAngularImpulse(ref impulse); } return(Math.Abs(lambda)); }
private bool IsContactUnique(ref ContactData contactCandidate, ref QuickList <ContactData> candidatesToAdd) { contactCandidate.Validate(); float distanceSquared; RigidTransform meshTransform = MeshTransform; for (int i = 0; i < contacts.Count; i++) { Vector3Ex.DistanceSquared(ref contacts.Elements[i].Position, ref contactCandidate.Position, out distanceSquared); if (distanceSquared < CollisionDetectionSettings.ContactMinimumSeparationDistanceSquared) { //This is a nonconvex manifold. There will be times where a an object will be shoved into a corner such that //a single position will have two reasonable normals. If the normals aren't mostly aligned, they should NOT be considered equivalent. Vector3Ex.Dot(ref contacts.Elements[i].Normal, ref contactCandidate.Normal, out distanceSquared); if (Math.Abs(distanceSquared) >= CollisionDetectionSettings.nonconvexNormalDotMinimum) { //Update the existing 'redundant' contact with the new information. //This works out because the new contact is the deepest contact according to the previous collision detection iteration. contacts.Elements[i].Normal = contactCandidate.Normal; contacts.Elements[i].Position = contactCandidate.Position; contacts.Elements[i].PenetrationDepth = contactCandidate.PenetrationDepth; supplementData.Elements[i].BasePenetrationDepth = contactCandidate.PenetrationDepth; RigidTransform.TransformByInverse(ref contactCandidate.Position, ref convex.worldTransform, out supplementData.Elements[i].LocalOffsetA); RigidTransform.TransformByInverse(ref contactCandidate.Position, ref meshTransform, out supplementData.Elements[i].LocalOffsetB); return(false); } } } for (int i = 0; i < candidatesToAdd.Count; i++) { Vector3Ex.DistanceSquared(ref candidatesToAdd.Elements[i].Position, ref contactCandidate.Position, out distanceSquared); if (distanceSquared < CollisionDetectionSettings.ContactMinimumSeparationDistanceSquared) { //This is a nonconvex manifold. There will be times where a an object will be shoved into a corner such that //a single position will have two reasonable normals. If the normals aren't mostly aligned, they should NOT be considered equivalent. Vector3Ex.Dot(ref candidatesToAdd.Elements[i].Normal, ref contactCandidate.Normal, out distanceSquared); if (Math.Abs(distanceSquared) >= CollisionDetectionSettings.nonconvexNormalDotMinimum) { return(false); } } } //for (int i = 0; i < edgeContacts.count; i++) //{ // Vector3Ex.DistanceSquared(ref edgeContacts.Elements[i].ContactData.Position, ref contactCandidate.Position, out distanceSquared); // if (distanceSquared < CollisionDetectionSettings.ContactMinimumSeparationDistanceSquared) // { // return false; // } //} //for (int i = 0; i < vertexContacts.count; i++) //{ // Vector3Ex.DistanceSquared(ref vertexContacts.Elements[i].ContactData.Position, ref contactCandidate.Position, out distanceSquared); // if (distanceSquared < CollisionDetectionSettings.ContactMinimumSeparationDistanceSquared) // { // return false; // } //} return(true); }
///<summary> /// Gets the closest point on the segment to the origin. ///</summary> ///<param name="point">Closest point.</param> public void GetPointOnSegmentClosestToOrigin(out System.Numerics.Vector3 point) { System.Numerics.Vector3 segmentDisplacement; Vector3Ex.Subtract(ref B, ref A, out segmentDisplacement); float dotA; Vector3Ex.Dot(ref segmentDisplacement, ref A, out dotA); //Inside segment. float V = -dotA / segmentDisplacement.LengthSquared(); Vector3Ex.Multiply(ref segmentDisplacement, V, out point); Vector3Ex.Add(ref point, ref A, out point); //if (dotB > 0) //{ //} //else //{ // //It is not possible to be anywhere but within the segment 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; // State = SimplexState.Point; // point = A; //} //It can never be outside A! //That would mean that the origin is LESS extreme along the search direction than our extreme point--- our search direction would not have picked that direction. }
private Ray GetReflectionRay(Vector3 P, Vector3 N, Vector3 V) { double c1 = -Vector3Ex.Dot(N, V); Vector3 Rl = V + (N * 2 * c1); return(new Ray(P, Rl, Ray.sameSurfaceOffset, double.MaxValue)); }
protected internal override void UpdateJacobiansAndVelocityBias() { linearJacobian = new Matrix3x3(); System.Numerics.Vector3 boneAxis; QuaternionEx.Transform(ref BoneLocalFreeAxis, ref TargetBone.Orientation, out boneAxis); angularJacobian = new Matrix3x3 { M11 = constrainedAxis1.X, M12 = constrainedAxis1.Y, M13 = constrainedAxis1.Z, M21 = constrainedAxis2.X, M22 = constrainedAxis2.Y, M23 = constrainedAxis2.Z }; System.Numerics.Vector3 error; Vector3Ex.Cross(ref boneAxis, ref freeAxis, out error); System.Numerics.Vector2 constraintSpaceError; Vector3Ex.Dot(ref error, ref constrainedAxis1, out constraintSpaceError.X); Vector3Ex.Dot(ref error, ref constrainedAxis2, out constraintSpaceError.Y); velocityBias.X = errorCorrectionFactor * constraintSpaceError.X; velocityBias.Y = errorCorrectionFactor * constraintSpaceError.Y; }
///<summary> /// Updates the time of impact for the pair. ///</summary> ///<param name="requester">Collidable requesting the update.</param> ///<param name="dt">Timestep duration.</param> public override void UpdateTimeOfImpact(Collidable requester, float dt) { //Notice that we don't test for convex entity null explicitly. The convex.IsActive property does that for us. if (convex.IsActive && convex.entity.PositionUpdateMode == PositionUpdateMode.Continuous) { //TODO: This system could be made more robust by using a similar region-based rejection of edges. //CCD events are awfully rare under normal circumstances, so this isn't usually an issue. //Only perform the test if the minimum radii are small enough relative to the size of the velocity. System.Numerics.Vector3 velocity; Vector3Ex.Multiply(ref convex.entity.linearVelocity, dt, out velocity); float velocitySquared = velocity.LengthSquared(); var minimumRadius = convex.Shape.MinimumRadius * MotionSettings.CoreShapeScaling; timeOfImpact = 1; if (minimumRadius * minimumRadius < velocitySquared) { var triangle = PhysicsThreadResources.GetTriangle(); triangle.collisionMargin = 0; //Spherecast against all triangles to find the earliest time. for (int i = 0; i < MeshManifold.overlappedTriangles.Count; i++) { mesh.Shape.TriangleMeshData.GetTriangle(MeshManifold.overlappedTriangles.Elements[i], out triangle.vA, out triangle.vB, out triangle.vC); //Put the triangle into 'localish' space of the convex. Vector3Ex.Subtract(ref triangle.vA, ref convex.worldTransform.Position, out triangle.vA); Vector3Ex.Subtract(ref triangle.vB, ref convex.worldTransform.Position, out triangle.vB); Vector3Ex.Subtract(ref triangle.vC, ref convex.worldTransform.Position, out triangle.vC); RayHit rayHit; if (GJKToolbox.CCDSphereCast(new Ray(Toolbox.ZeroVector, velocity), minimumRadius, triangle, ref Toolbox.RigidIdentity, timeOfImpact, out rayHit) && rayHit.T > Toolbox.BigEpsilon) { if (mesh.sidedness != TriangleSidedness.DoubleSided) { System.Numerics.Vector3 AB, AC; Vector3Ex.Subtract(ref triangle.vB, ref triangle.vA, out AB); Vector3Ex.Subtract(ref triangle.vC, ref triangle.vA, out AC); System.Numerics.Vector3 normal; Vector3Ex.Cross(ref AB, ref AC, out normal); float dot; Vector3Ex.Dot(ref normal, ref rayHit.Normal, out dot); //Only perform sweep if the object is in danger of hitting the object. //Triangles can be one sided, so check the impact normal against the triangle normal. if (mesh.sidedness == TriangleSidedness.Counterclockwise && dot < 0 || mesh.sidedness == TriangleSidedness.Clockwise && dot > 0) { timeOfImpact = rayHit.T; } } else { timeOfImpact = rayHit.T; } } } PhysicsThreadResources.GiveBack(triangle); } } }
/// <summary> /// Computes closest distance from a vertex to a plane /// </summary> /// <param name="vertex">vertex used to compute the distance</param> /// <param name="face">face representing the plane where it is contained</param> /// <returns>the closest distance from the vertex to the plane</returns> public double DistanceFromPlane(Vertex vertex) { double distToV1 = this.Plane.DistanceFromOrigin; double distToVertex = Vector3Ex.Dot(Normal, vertex.Position); double distFromFacePlane = distToVertex - distToV1; return(distFromFacePlane); }
public void DotProduct() { var test1 = new Vector3(10, 1, 2); var test2 = new Vector3(1, 0, 0); double dotResult = Vector3Ex.Dot(test2, test1); Assert.IsTrue(dotResult == 10); }
public void DotProduct() { Vector3 Test1 = new Vector3(10, 1, 2); Vector3 Test2 = new Vector3(1, 0, 0); double DotResult = Vector3Ex.Dot(Test2, Test1); Assert.IsTrue(DotResult == 10); }
//public static void GetInertiaOffset(System.Numerics.Vector3 offset, float mass, out Matrix3x3 additionalInertia) //{ // additionalInertia.M11 = mass * (offset.Y * offset.Y + offset.Z * offset.Z); // additionalInertia.M12 = -mass * offset.X * offset.Y; // additionalInertia.M13 = -mass * offset.X * offset.Z; // additionalInertia.M21 = -mass * offset.Y * offset.X; // additionalInertia.M22 = mass * (offset.X * offset.X + offset.Z * offset.Z); // additionalInertia.M23 = -mass * offset.Y * offset.Z; // additionalInertia.M31 = -mass * offset.Z * offset.X; // additionalInertia.M32 = -mass * offset.Z * offset.Y; // additionalInertia.M33 = mass * (offset.X * offset.X + offset.Y * offset.Y); //} /// <summary> /// Computes a minimum radius estimate of a shape based on a convex mesh representation. /// </summary> /// <param name="vertices">Vertices of the convex mesh.</param> /// <param name="triangleIndices">Groups of 3 indices into the vertices array which represent the triangles of the convex mesh.</param> /// <param name="center">Center of the convex shape.</param> public static float ComputeMinimumRadius(IList <System.Numerics.Vector3> vertices, IList <int> triangleIndices, ref System.Numerics.Vector3 center) { //Walk through all of the triangles. Treat them as a bunch of planes which bound the shape. //The closest distance on any of those planes to the center is the radius of the largest sphere, //centered on the... center, which can fit in the shape. //While this shares a lot of math with the volume distribution computation (volume of a parallelepiped), //it requires that a center be available. So, it's a separate calculation. float minimumDistance = float.MaxValue; for (int i = 0; i < triangleIndices.Count; i += 3) { System.Numerics.Vector3 v2 = vertices[triangleIndices[i]]; System.Numerics.Vector3 v3 = vertices[triangleIndices[i + 1]]; System.Numerics.Vector3 v4 = vertices[triangleIndices[i + 2]]; //This normal calculation creates a dependency on winding. //It needs to be consistent with the SampleDirections triangle winding. System.Numerics.Vector3 v2v3, v2v4; Vector3Ex.Subtract(ref v3, ref v2, out v2v3); Vector3Ex.Subtract(ref v4, ref v2, out v2v4); System.Numerics.Vector3 normal; Vector3Ex.Cross(ref v2v4, ref v2v3, out normal); //Watch out: this could very easily be a degenerate triangle; the sampling approach tends to create them. float lengthSquared = normal.LengthSquared(); if (lengthSquared > 1e-10f) { Vector3Ex.Divide(ref normal, (float)Math.Sqrt(lengthSquared), out normal); } else { continue; } System.Numerics.Vector3 fromCenterToPlane; Vector3Ex.Subtract(ref v2, ref center, out fromCenterToPlane); float distance; Vector3Ex.Dot(ref normal, ref fromCenterToPlane, out distance); if (distance < 0) { throw new ArgumentException("Invalid distance. Ensure the mesh is convex, has consistent winding, and contains the passed-in center."); } if (distance < minimumDistance) { minimumDistance = distance; } } return(minimumDistance); //Technically, we could also compute a maximum radius estimate... //but that amounts to finding the furthest distance contained by the set of planes defined by the sampled extreme points and their associated sample directions. //That's a trickier thing to compute quickly, and it's not all that important to make the estimate ultra tight. }
///<summary> /// Updates the manifold. ///</summary> ///<param name="dt">Timestep duration.</param> public override void Update(float dt) { //First, refresh all existing contacts. This is an incremental manifold. ContactRefresher.ContactRefresh(contacts, supplementData, ref collidableA.worldTransform, ref collidableB.worldTransform, contactIndicesToRemove); RemoveQueuedContacts(); //Now, generate a contact between the two shapes. ContactData contact; if (pairTester.GenerateContactCandidate(out contact)) { //Eliminate any old contacts which have normals which would fight with this new contact. for (int i = 0; i < contacts.Count; ++i) { float normalDot; Vector3Ex.Dot(ref contacts.Elements[i].Normal, ref contact.Normal, out normalDot); if (normalDot < 0) { Remove(i); break; } } //If a contact is unique, add it to the manifold separately. //If it is redundant, it will be used to update an existing contact... within the IsContactUnique call. //In other words: THIS FUNCTION HAS IMPORTANT SNEAKY SIDE EFFECTS. if (IsContactUnique(ref contact)) { //Check if adding the new contact would overflow the manifold. if (contacts.Count == 4) { //Adding that contact would overflow the manifold. Reduce to the best subset. bool addCandidate; ContactReducer.ReduceContacts(contacts, ref contact, contactIndicesToRemove, out addCandidate); RemoveQueuedContacts(); if (addCandidate) { Add(ref contact); } } else { //Won't overflow the manifold, so just toss it in. Add(ref contact); } } } else { //No collision, clean out the manifold. for (int i = contacts.Count - 1; i >= 0; i--) { Remove(i); } } }
/// <summary> /// Constructs a new constraint which restricts three degrees of linear freedom and two degrees of angular freedom between two entities. /// </summary> /// <param name="connectionA">First entity of the constraint pair.</param> /// <param name="connectionB">Second entity of the constraint pair.</param> /// <param name="anchor">Point around which both entities rotate.</param> /// <param name="freeAxis">Axis around which the hinge can rotate.</param> public RevoluteJoint(Entity connectionA, Entity connectionB, System.Numerics.Vector3 anchor, System.Numerics.Vector3 freeAxis) { if (connectionA == null) { connectionA = TwoEntityConstraint.WorldEntity; } if (connectionB == null) { connectionB = TwoEntityConstraint.WorldEntity; } BallSocketJoint = new BallSocketJoint(connectionA, connectionB, anchor); AngularJoint = new RevoluteAngularJoint(connectionA, connectionB, freeAxis); Limit = new RevoluteLimit(connectionA, connectionB); Motor = new RevoluteMotor(connectionA, connectionB, freeAxis); Limit.IsActive = false; Motor.IsActive = false; //Ensure that the base and test direction is perpendicular to the free axis. System.Numerics.Vector3 baseAxis = anchor - connectionA.position; if (baseAxis.LengthSquared() < Toolbox.BigEpsilon) //anchor and connection a in same spot, so try the other way. { baseAxis = connectionB.position - anchor; } baseAxis -= Vector3Ex.Dot(baseAxis, freeAxis) * freeAxis; if (baseAxis.LengthSquared() < Toolbox.BigEpsilon) { //However, if the free axis is totally aligned (like in an axis constraint), pick another reasonable direction. baseAxis = System.Numerics.Vector3.Cross(freeAxis, Vector3Ex.Up); if (baseAxis.LengthSquared() < Toolbox.BigEpsilon) { baseAxis = System.Numerics.Vector3.Cross(freeAxis, Vector3Ex.Right); } } Limit.Basis.SetWorldAxes(freeAxis, baseAxis, connectionA.orientationMatrix); Motor.Basis.SetWorldAxes(freeAxis, baseAxis, connectionA.orientationMatrix); baseAxis = connectionB.position - anchor; baseAxis -= Vector3Ex.Dot(baseAxis, freeAxis) * freeAxis; if (baseAxis.LengthSquared() < Toolbox.BigEpsilon) { //However, if the free axis is totally aligned (like in an axis constraint), pick another reasonable direction. baseAxis = System.Numerics.Vector3.Cross(freeAxis, Vector3Ex.Up); if (baseAxis.LengthSquared() < Toolbox.BigEpsilon) { baseAxis = System.Numerics.Vector3.Cross(freeAxis, Vector3Ex.Right); } } Limit.TestAxis = baseAxis; Motor.TestAxis = baseAxis; Add(BallSocketJoint); Add(AngularJoint); Add(Limit); Add(Motor); }
void AnalyzeEntry(int i) { var entityCollidable = broadPhaseEntries[i] as EntityCollidable; if (entityCollidable != null && entityCollidable.IsActive && entityCollidable.entity.isDynamic && CollisionRules.collisionRuleCalculator(this, entityCollidable) <= CollisionRule.Normal) { bool keepGoing = false; foreach (var tri in surfaceTriangles) { //Don't need to do anything if the entity is outside of the water. if (Toolbox.IsPointInsideTriangle(ref tri[0], ref tri[1], ref tri[2], ref entityCollidable.worldTransform.Position)) { keepGoing = true; break; } } if (!keepGoing) { return; } //The entity is submerged, apply buoyancy forces. float submergedVolume; System.Numerics.Vector3 submergedCenter; GetBuoyancyInformation(entityCollidable, out submergedVolume, out submergedCenter); if (submergedVolume > 0) { float fractionSubmerged = submergedVolume / entityCollidable.entity.CollisionInformation.Shape.Volume; //Divide the volume by the density multiplier if present. float densityMultiplier; if (DensityMultipliers.TryGetValue(entityCollidable.entity, out densityMultiplier)) { submergedVolume /= densityMultiplier; } System.Numerics.Vector3 force; Vector3Ex.Multiply(ref upVector, -gravity * Density * dt * submergedVolume, out force); entityCollidable.entity.ApplyImpulse(ref submergedCenter, ref force); //Flow if (FlowForce != 0) { float dot = Math.Max(Vector3Ex.Dot(entityCollidable.entity.linearVelocity, flowDirection), 0); if (dot < MaxFlowSpeed) { force = Math.Min(FlowForce, (MaxFlowSpeed - dot) * entityCollidable.entity.mass) * dt * fractionSubmerged * FlowDirection; entityCollidable.entity.ApplyLinearImpulse(ref force); } } //Damping entityCollidable.entity.ModifyLinearDamping(fractionSubmerged * LinearDamping); entityCollidable.entity.ModifyAngularDamping(fractionSubmerged * AngularDamping); } } }
/// <summary> /// Sets up the axes of the transform and ensures that it is an orthonormal basis. /// </summary> /// <param name="matrix">Rotation matrix representing the three axes. /// The matrix's backward vector is used as the primary axis. /// The matrix's right vector is used as the x axis.</param> public void SetLocalAxes(Matrix3x3 matrix) { if (Math.Abs(Vector3Ex.Dot(matrix.Backward, matrix.Right)) > Toolbox.BigEpsilon) { throw new ArgumentException("The axes provided to the joint transform are not perpendicular. Ensure that the specified axes form a valid constraint."); } localPrimaryAxis = System.Numerics.Vector3.Normalize(matrix.Backward); localXAxis = System.Numerics.Vector3.Normalize(matrix.Right); ComputeWorldSpaceAxes(); }
/// <summary> /// Performs any per-frame computation needed by the constraint. /// </summary> /// <param name="dt">Time step duration.</param> public override void Update(float dt) { //Collect references, pick the mode, and configure the coefficients to be used by the solver. if (supportData.SupportObject != null) { //Get an easy reference to the support. var support = supportData.SupportObject as EntityCollidable; if (support != null) { supportEntity = support.Entity; } else { supportEntity = null; } } else { supportEntity = null; } maximumForce = maximumGlueForce * dt; //If we don't allow the character to get out of the ground, it could apply some significant forces to a dynamic support object. //Let the character escape penetration in a controlled manner. This mirrors the regular penetration recovery speed. //Since the vertical motion constraint works in the opposite direction of the contact penetration constraint, //this actually eliminates the 'bounce' that can occur with non-character objects in deep penetration. permittedVelocity = Math.Min(Math.Max(supportData.Depth * CollisionResponseSettings.PenetrationRecoveryStiffness / dt, 0), CollisionResponseSettings.MaximumPenetrationRecoverySpeed); //Compute the jacobians and effective mass matrix. This constraint works along a single degree of freedom, so the mass matrix boils down to a scalar. linearJacobianA = supportData.Normal; Vector3Ex.Negate(ref linearJacobianA, out linearJacobianB); float inverseEffectiveMass = characterBody.InverseMass; if (supportEntity != null) { System.Numerics.Vector3 offsetB = supportData.Position - supportEntity.Position; Vector3Ex.Cross(ref offsetB, ref linearJacobianB, out angularJacobianB); if (supportEntity.IsDynamic) { //Only dynamic entities can actually contribute anything to the effective mass. //Kinematic entities have infinite mass and inertia, so this would all zero out. Matrix3x3 inertiaInverse = supportEntity.InertiaTensorInverse; System.Numerics.Vector3 angularComponentB; Matrix3x3.Transform(ref angularJacobianB, ref inertiaInverse, out angularComponentB); float effectiveMassContribution; Vector3Ex.Dot(ref angularComponentB, ref angularJacobianB, out effectiveMassContribution); inverseEffectiveMass += supportForceFactor * (effectiveMassContribution + supportEntity.InverseMass); } } effectiveMass = 1f / (inverseEffectiveMass); //So much nicer and shorter than the horizontal constraint! }
// For right cylinders, SetCenter is the center point. For non-right // cylinders, the center is just any point on the central axis. void SetCenter(Vector3 center) { this.Center = center; CenterDotAxis = Vector3Ex.Dot(Center, CenterAxis); if (IsRightCylinder()) { TopPlaneCoef = CenterDotAxis + HalfHeight; BottomPlaneCoef = -(CenterDotAxis - HalfHeight); } }
public override (double u, double v) GetUv(IntersectInfo info) { Vector3 Position = plane.Normal; Vector3 vecU = new Vector3(Position.Y, Position.Z, -Position.X); Vector3 vecV = Vector3Ex.Cross(vecU, plane.Normal); double u = Vector3Ex.Dot(info.HitPosition, vecU); double v = Vector3Ex.Dot(info.HitPosition, vecV); return(u, v); }