public static bool ClosestPoints(ISupportMappable support1, ISupportMappable support2, ref JMatrix orientation1, ref JMatrix orientation2, ref JVector position1, ref JVector position2, out JVector p1, out JVector p2, out JVector normal) { VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew(); simplexSolver.Reset(); p1 = p2 = JVector.Zero; JVector r = position1 - position2; JVector w, v; JVector supVertexA; JVector rn, vn; rn = JVector.Negate(r); SupportMapTransformed(support1, ref orientation1, ref position1, ref rn, out supVertexA); JVector supVertexB; SupportMapTransformed(support2, ref orientation2, ref position2, ref r, out supVertexB); v = supVertexA - supVertexB; normal = JVector.Zero; int iter = 0; float distSq = v.LengthSquared(); float epsilon = 0.00001f; while ((distSq > epsilon) && (iter++ < MaxIterations)) { IterationsTaken = iter; vn = JVector.Negate(v); SupportMapTransformed(support1, ref orientation1, ref position1, ref vn, out supVertexA); SupportMapTransformed(support2, ref orientation2, ref position2, ref v, out supVertexB); w = supVertexA - supVertexB; if (!simplexSolver.InSimplex(w)) simplexSolver.AddVertex(w, supVertexA, supVertexB); if (simplexSolver.Closest(out v)) { distSq = v.LengthSquared(); normal = v; } else distSq = 0.0f; } simplexSolver.ComputePoints(out p1, out p2); if (normal.LengthSquared() > JMath.Epsilon * JMath.Epsilon) normal.Normalize(); simplexSolverPool.GiveBack(simplexSolver); return true; }
public FixedLinearSpring(RigidBody body, JVector localAnchor, JVector worldAnchor, float springConstant, float dampingConstant) { Body = body; LocalAnchor = localAnchor; WorldAnchor = worldAnchor; SpringConstant = springConstant; DampingConstant = dampingConstant; Length = (worldAnchor - Body.LocalToWorld(localAnchor)).Length(); }
/// <summary> /// Discrete Box vs Box test. Very fast. No contact info. NOTE: ensure UpdateAxes is called for each BoxShape prior. /// </summary> /// <param name="A">BoxShape A.</param> /// <param name="PA">BoxShape A's position.</param> /// <param name="B">BoxShape B.</param> /// <param name="PB">BoxShape B's position.</param> /// <returns></returns> public static bool BoxBoxTest(ref BoxShape A, ref JVector PA, ref BoxShape B, ref JVector PB) { JVector T = PB - PA; // cache scaled local axes JVector Ax = A.halfSize.X * A.xAxis; JVector Ay = A.halfSize.Y * A.yAxis; JVector Bx = B.halfSize.X * B.xAxis; JVector By = B.halfSize.Y * B.yAxis; // axis to test JVector L = A.xAxis; float TL = Math.Abs(T * L); float a = Math.Abs((Ax) * L) + Math.Abs((Ay) * L); float b = Math.Abs((Bx) * L) + Math.Abs((By) * L); if (TL > (a + b)) return false; // axis to test L = A.yAxis; TL = Math.Abs(T * L); a = Math.Abs((Ax) * L) + Math.Abs((Ay) * L); b = Math.Abs((Bx) * L) + Math.Abs((By) * L); if (TL > (a + b)) return false; // axis to test L = B.xAxis; TL = Math.Abs(T * L); a = Math.Abs((Ax) * L) + Math.Abs((Ay) * L); b = Math.Abs((Bx) * L) + Math.Abs((By) * L); if (TL > (a + b)) return false; // axis to test L = B.yAxis; TL = Math.Abs(T * L); a = Math.Abs((Ax) * L) + Math.Abs((Ay) * L); b = Math.Abs((Bx) * L) + Math.Abs((By) * L); if (TL > (a + b)) return false; // if all axes overlap collision exists return true; }
public static void SupportMapTransformed(ISupportMappable support, ref JMatrix orientation, ref JVector position, ref JVector direction, out JVector result) { // THIS IS *THE* HIGH FREQUENCY CODE OF THE COLLLISION PART OF THE ENGINE result.X = ((direction.X * orientation.M11) + (direction.Y * orientation.M12)); result.Y = ((direction.X * orientation.M21) + (direction.Y * orientation.M22)); support.SupportMapping(ref result, out result); float x = ((result.X * orientation.M11) + (result.Y * orientation.M21)); float y = ((result.X * orientation.M12) + (result.Y * orientation.M22)); result.X = position.X + x; result.Y = position.Y + y; }
/// <summary> /// SupportMapping. Finds the point in the shape furthest away from the given direction. /// Imagine a plane with a normal in the search direction. Now move the plane along the normal /// until the plane does not intersect the shape. The last intersection point is the result. /// </summary> /// <param name="direction">The direction.</param> /// <param name="result">The result.</param> public override void SupportMapping(ref JVector direction, out JVector result) { float maxDotProduct = float.MinValue; int maxIndex = 0; float dotProduct; for (int i = 0; i < vertices.Count; i++) { dotProduct = JVector.Dot(vertices[i], direction); if (dotProduct > maxDotProduct) { maxDotProduct = dotProduct; maxIndex = i; } } result = vertices[maxIndex] - this.shifted; }
public static bool CircleCircleTest(JVector centerA, float radiusA, JVector centerB, float radiusB, out JVector pointA, out JVector pointB, out JVector normal, out float distance) { // ||A-B|| - (r1+r2) < 0 float d = JVector.Distance(centerA, centerB); float r = radiusA + radiusB; distance = d - r; normal = (centerA - centerB) / d; //penetrationVector = normal * distance; // calculate closest 2 points pointA = JVector.Negate(normal) * radiusA + centerA; pointB = normal * radiusB + centerB; if (distance < 0.0f) return true; else return false; }
/// <summary> /// Transforms the bounding box into the space given by orientation and position. /// </summary> /// <param name="position"></param> /// <param name="orientation"></param> /// <param name="result"></param> internal void InverseTransform(ref JVector position, ref JMatrix orientation) { JVector.Subtract(ref Max, ref position, out Max); JVector.Subtract(ref Min, ref position, out Min); JVector center; JVector.Add(ref Max, ref Min, out center); center.X *= 0.5f; center.Y *= 0.5f; JVector halfExtents; JVector.Subtract(ref Max, ref Min, out halfExtents); halfExtents.X *= 0.5f; halfExtents.Y *= 0.5f; JVector.TransposedTransform(ref center, ref orientation, out center); JMatrix abs; JMath.Absolute(ref orientation, out abs); JVector.TransposedTransform(ref halfExtents, ref abs, out halfExtents); JVector.Add(ref center, ref halfExtents, out Max); JVector.Subtract(ref center, ref halfExtents, out Min); }
public static bool CircleCapsuleTest(JVector centerA, float radiusA, JVector centerB, JVector axis, float length, float radiusB, out JVector pointA, out JVector pointB, out JVector normal, out float distance) { // get capsule endpoints var p0 = centerB - axis * (length * 0.5f); var p1 = centerB + axis * (length * 0.5f); // get vector from endpoint to circle var D = centerA - p0; // project vector onto axis and clamp var d = JVector.Dot(D, axis); d = JMath.Clamp(d, 0, length); // get point on axis var R = p0 + axis * d; // distance var b = Math.Abs((centerA - R).Length()); normal = (centerA - R) / b; // calculate closest 2 points var RH = JVector.Normalize(centerA - R); pointA = JVector.Negate(RH) * radiusA + centerA; pointB = RH * radiusB + R; normal.Negate(); distance = b - (radiusA + radiusB); // if (b < radiusA + radiusB) return true; return false; }
public void AddPoint(JVector point) { AddPoint(ref point); }
public bool SegmentIntersect(JVector origin, JVector direction) { return(SegmentIntersect(ref origin, ref direction)); }
/// <summary> /// Checks wether a point is within a box or not. /// </summary> /// <param name="point"></param> /// <returns></returns> public ContainmentType Contains(JVector point) { return(this.Contains(ref point)); }
/// <summary> /// Transforms a vector by the transposed of the given Matrix. /// </summary> /// <param name="position">The vector to transform.</param> /// <param name="matrix">The transform matrix.</param> /// <param name="result">The transformed vector.</param> public static void TransposedTransform(ref JVector position, ref JMatrix matrix, out JVector result) { float num0 = ((position.X * matrix.M11) + (position.Y * matrix.M12)); float num1 = ((position.X * matrix.M21) + (position.Y * matrix.M22)); result.X = num0; result.Y = num1; }
static JVector() { One = new JVector(1, 1); Zero = new JVector(0, 0); Left = new JVector(1, 0); Right = new JVector(-1, 0); Up = new JVector(0, 1); Down = new JVector(0, -1); MinValue = new JVector(float.MinValue); MaxValue = new JVector(float.MaxValue); Arbitrary = new JVector(1, 1); InternalZero = Zero; }
/// <summary> /// Subtracts to vectors. /// </summary> /// <param name="value1">The first vector.</param> /// <param name="value2">The second vector.</param> /// <param name="result">The difference of both vectors.</param> public static void Subtract(ref JVector value1, ref JVector value2, out JVector result) { float num0 = value1.X - value2.X; float num1 = value1.Y - value2.Y; result.X = num0; result.Y = num1; }
public static JVector Transform(JVector position, JMatrix matrix) { JVector result; JVector.Transform(ref position, ref matrix, out result); return result; }
/// <summary> /// Adds two vectors. /// </summary> /// <param name="value1">The first vector.</param> /// <param name="value2">The second vector.</param> /// <returns>The sum of both vectors.</returns> #region public static JVector operator +(JVector value1, JVector value2) public static JVector operator +(JVector value1, JVector value2) { JVector result; JVector.Add(ref value1, ref value2, out result); return(result); }
public static JVector Cross(JVector a, float s) { return(new JVector(s * a.Y, -s * a.X)); }
/// <summary> /// Calculates the dot product of two vectors. /// </summary> /// <param name="value1">The first vector.</param> /// <param name="value2">The second vector.</param> /// <returns>Returns the dot product of both.</returns> #region public static float operator *(JVector value1, JVector value2) public static float operator *(JVector value1, JVector value2) { return(JVector.Dot(ref value1, ref value2)); }
/// <summary> /// Subtracts two vectors. /// </summary> /// <param name="value1">The first vector.</param> /// <param name="value2">The second vector.</param> /// <returns>The difference of both vectors.</returns> #region public static JVector operator -(JVector value1, JVector value2) public static JVector operator -(JVector value1, JVector value2) { JVector result; JVector.Subtract(ref value1, ref value2, out result); return(result); }
/// <summary> /// Calculates the cross product of two vectors. /// </summary> /// <param name="value1">The first vector.</param> /// <param name="value2">The second vector.</param> /// <returns>Returns the cross product of both.</returns> #region public static JVector operator %(JVector value1, JVector value2) public static float operator %(JVector value1, JVector value2) { float result; JVector.Cross(ref value1, ref value2, out result); return(result); }
/// <summary> /// Multiply a vector with a factor. /// </summary> /// <param name="value1">The vector to multiply.</param> /// <param name="scaleFactor">The scale factor.</param> /// <param name="result">Returns the multiplied vector.</param> public static void Multiply(ref JVector value1, float scaleFactor, out JVector result) { result.X = value1.X * scaleFactor; result.Y = value1.Y * scaleFactor; }
/// <summary> /// Calculates the dot product of both vectors. /// </summary> /// <param name="vector1">The first vector.</param> /// <param name="vector2">The second vector.</param> /// <returns>Returns the dot product of both vectors.</returns> public static float Dot(ref JVector vector1, ref JVector vector2) { return((vector1.X * vector2.X) + (vector1.Y * vector2.Y)); }
///// <summary> ///// Returns the largest box possible. ///// </summary> //public static readonly JBBox LargeBox; ///// <summary> ///// Returns the smalltest box possible. ///// </summary> //public static readonly JBBox SmallBox; //static JBBox() //{ // LargeBox.Min = new JVector(float.MinValue); // LargeBox.Max = new JVector(float.MaxValue); // SmallBox.Min = new JVector(float.MaxValue); // SmallBox.Max = new JVector(float.MinValue); //} /// <summary> /// Constructor /// </summary> /// <param name="min">The minimum point of the box.</param> /// <param name="max">The maximum point of the box.</param> public JBBox(JVector min, JVector max) { this.Min = min; this.Max = max; }
public static JVector Cross(float s, JVector a) { return(new JVector(-s * a.Y, s * a.X)); }
/// <summary> /// Calculates the dot product of two vectors. /// </summary> /// <param name="vector1">The first vector.</param> /// <param name="vector2">The second vector.</param> /// <returns>Returns the dot product of both vectors.</returns> #region public static float Dot(JVector vector1, JVector vector2) public static float Dot(JVector vector1, JVector vector2) { return(JVector.Dot(ref vector1, ref vector2)); }
internal static JVector Clamp(JVector v, float len) { return((JVector.Dot(v, v) > len * len) ? JVector.Multiply(JVector.Normalize(v), len) : v); }
/// <summary> /// Swaps the components of both vectors. /// </summary> /// <param name="vector1">The first vector to swap with the second.</param> /// <param name="vector2">The second vector to swap with the first.</param> public static void Swap(ref JVector vector1, ref JVector vector2) { float temp; temp = vector1.X; vector1.X = vector2.X; vector2.X = temp; temp = vector1.Y; vector1.Y = vector2.Y; vector2.Y = temp; }
public static JVector Multiply(JVector value1, float scaleFactor) { JVector result; JVector.Multiply(ref value1, scaleFactor, out result); return result; }
/// <summary> /// Transforms a vector by the transposed of the given Matrix. /// </summary> /// <param name="position">The vector to transform.</param> /// <param name="matrix">The transform matrix.</param> /// <param name="result">The transformed vector.</param> public static JVector TransposedTransform(JVector position, JMatrix matrix) { JVector result; float num0 = ((position.X * matrix.M11) + (position.Y * matrix.M12)); float num1 = ((position.X * matrix.M21) + (position.Y * matrix.M22)); result.X = num0; result.Y = num1; return result; }
internal static JVector Clamp(JVector v, float len) { return (JVector.Dot(v, v) > len * len) ? JVector.Multiply(JVector.Normalize(v), len) : v; }
public override bool Raycast(JVector rayOrigin, JVector rayDirection, RaycastCallback raycast, out RigidBody body, out JVector normal, out float fraction) { throw new NotImplementedException(); //body = null; normal = JVector.Zero; fraction = float.MaxValue; //JVector tempNormal; float tempFraction; //bool result = false; //// TODO: This can be done better in CollisionSystemPersistenSAP //foreach (IBroadphaseEntity e in bodyList) //{ // if (e is SoftBody) // { // SoftBody softBody = e as SoftBody; // foreach (RigidBody b in softBody.VertexBodies) // { // if (this.Raycast(b, rayOrigin, rayDirection, out tempNormal, out tempFraction)) // { // if (tempFraction < fraction && (raycast == null || raycast(b, tempNormal, tempFraction))) // { // body = b; // normal = tempNormal; // fraction = tempFraction; // result = true; // } // } // } // } // else // { // RigidBody b = e as RigidBody; // if (this.Raycast(b, rayOrigin, rayDirection, out tempNormal, out tempFraction)) // { // if (tempFraction < fraction && (raycast == null || raycast(b, tempNormal, tempFraction))) // { // body = b; // normal = tempNormal; // fraction = tempFraction; // result = true; // } // } // } //} //return result; }
public JVector ScreenToWorldSpace(JVector position) { var t = this.Game.GraphicsDevice.Viewport.Unproject(new Vector3(position.X, position.Y, 0), Projection, View, Matrix.Identity); return new JVector(t.X, t.Y); }
public static Vector2 ToXNAVector2(JVector vector) { return new Vector2(vector.X, vector.Y); }
public bool RayIntersect(JVector origin, JVector direction) { return(RayIntersect(ref origin, ref direction)); }
protected override void Update(GameTime gameTime) { // Allows the game to exit if (GamePad.GetState(PlayerIndex.One).Buttons.Back == ButtonState.Pressed) this.Exit(); KeyboardState keys = Keyboard.GetState(); JVector moveVector = JVector.Zero; float amountOfMovement = 0.05f; if (keys.IsKeyDown(Keys.Right)) moveVector.X += amountOfMovement; if (keys.IsKeyDown(Keys.Left)) moveVector.X -= amountOfMovement; if (keys.IsKeyDown(Keys.Down)) moveVector.Y -= amountOfMovement; if (keys.IsKeyDown(Keys.Up)) moveVector.Y += amountOfMovement; PB += moveVector; OA = (float)gameTime.TotalGameTime.TotalSeconds * 0.1f; OB = (float)gameTime.TotalGameTime.TotalSeconds * -0.1f; DrawBox(A, PA, OA, Color.Blue * 0.25f); DrawBox(B, PB, OB, Color.Green * 0.25f); float t = 0.0f; JVector[] CA = new JVector[2], CB = new JVector[2]; int NumContacts = 0; sw.Start(); JMatrix OAM = JMatrix.CreateRotationZ(OA); JMatrix OBM = JMatrix.CreateRotationZ(OB); for (int i = 0; i < 1; i++) { A.UpdateAxes(OA); B.UpdateAxes(OB); //hit = Collision.BoxBoxTest(ref A, ref PA, ref B, ref PB); //AV = new List<JVector> { A.GetCorner(0), A.GetCorner(1), A.GetCorner(2), A.GetCorner(3) }; //BV = new List<JVector> { B.GetCorner(0), B.GetCorner(1), B.GetCorner(2), B.GetCorner(3) }; //hit = SAT.Collide(ref AV, ref PA, ref VA, ref OAM, // ref BV, ref PB, ref VB, ref OBM, // ref normal, ref t); //if (hit) //{ // SAT.FindContacts(ref AV, ref PA, ref VA, ref OAM, // ref BV, ref PB, ref VB, ref OBM, // ref normal, t, out CA, out CB, out NumContacts); // normal.Normalize(); //} hit = Collision.BoxBoxTestContact(ref A, ref PA, ref OAM, ref B, ref PB, ref OBM, out normal, out t, out CA, out CB, out NumContacts); penetration = t; iterations = NumContacts; } sw.Stop(); ticks = sw.ElapsedTicks / 1; sw.Reset(); if (hit) { //DrawBox(A, PA + normal * (t * 0.5f), OA, Color.Blue); //DrawBox(B, PB - normal * (t * 0.5f), OB, Color.Green); for (int i = 0; i < NumContacts; i++) { DebugDrawer.DrawPoint(CA[i]);// + normal * (t * 0.5f)); DebugDrawer.DrawPoint(CB[i]);// - normal * (t * 0.5f)); } } base.Update(gameTime); }
/// <summary> /// Checks whether a point is inside, outside or intersecting /// a point. /// </summary> /// <param name="point">A point in space.</param> /// <returns>The ContainmentType of the point.</returns> public ContainmentType Contains(ref JVector point) { return((((this.Min.X <= point.X) && (point.X <= this.Max.X)) && ((this.Min.Y <= point.Y) && (point.Y <= this.Max.Y))) ? ContainmentType.Contains : ContainmentType.Disjoint); }
void DrawPoly(List<JVector> poly, JVector pos, JMatrix o, Color color) { for (int i = 0; i < poly.Count - 1; i++) { JVector a = JVector.Transform(poly[i], o * JMatrix.CreateTranslation(pos)); JVector b = JVector.Transform(poly[i + 1], o * JMatrix.CreateTranslation(pos)); DebugDrawer.DrawLine(a, b, color); } JVector c = JVector.Transform(poly[0], o * JMatrix.CreateTranslation(pos)); JVector d = JVector.Transform(poly[poly.Count - 1], o * JMatrix.CreateTranslation(pos)); DebugDrawer.DrawLine(c, d, color); }
public void AddPoint(ref JVector point) { JVector.Max(ref this.Max, ref point, out this.Max); JVector.Min(ref this.Min, ref point, out this.Min); }
/// <summary> /// Gets a vector with the maximum x and y values of both vectors. /// </summary> /// <param name="value1">The first value.</param> /// <param name="value2">The second value.</param> /// <param name="result">A vector with the maximum x and y values of both vectors.</param> public static void Max(ref JVector value1, ref JVector value2, out JVector result) { result.X = (value1.X > value2.X) ? value1.X : value2.X; result.Y = (value1.Y > value2.Y) ? value1.Y : value2.Y; }
/// <summary> /// Inverses the direction of a vector. /// </summary> /// <param name="value">The vector to inverse.</param> /// <returns>The negated vector.</returns> public static JVector Negate(JVector value) { JVector result; JVector.Negate(ref value, out result); return result; }
public override bool Raycast(RigidBody body, JVector rayOrigin, JVector rayDirection, out JVector normal, out float fraction) { throw new NotImplementedException(); //fraction = float.MaxValue; normal = JVector.Zero; //if (!body.BoundingBox.RayIntersect(ref rayOrigin, ref rayDirection)) return false; //if (body.Shape is Multishape) //{ // Multishape ms = (body.Shape as Multishape).RequestWorkingClone(); // JVector tempNormal; float tempFraction; // bool multiShapeCollides = false; // JVector transformedOrigin; JVector.Subtract(ref rayOrigin, ref body.position, out transformedOrigin); // JVector.Transform(ref transformedOrigin, ref body.invOrientation, out transformedOrigin); // JVector transformedDirection; JVector.Transform(ref rayDirection, ref body.invOrientation, out transformedDirection); // int msLength = ms.Prepare(ref transformedOrigin, ref transformedDirection); // for (int i = 0; i < msLength; i++) // { // ms.SetCurrentShape(i); // if (GJKCollide.Raycast(ms, ref body.orientation, ref body.invOrientation, ref body.position, // ref rayOrigin, ref rayDirection, out tempFraction, out tempNormal)) // { // if (tempFraction < fraction) // { // if (useTerrainNormal && ms is TerrainShape) // { // (ms as TerrainShape).CollisionNormal(out tempNormal); // JVector.Transform(ref tempNormal, ref body.orientation, out tempNormal); // tempNormal.Negate(); // } // else if (useTriangleMeshNormal && ms is TriangleMeshShape) // { // (ms as TriangleMeshShape).CollisionNormal(out tempNormal); // JVector.Transform(ref tempNormal, ref body.orientation, out tempNormal); // tempNormal.Negate(); // } // normal = tempNormal; // fraction = tempFraction; // multiShapeCollides = true; // } // } // } // ms.ReturnWorkingClone(); // return multiShapeCollides; //} //else //{ // return (GJKCollide.Raycast(body.Shape, ref body.orientation, ref body.invOrientation, ref body.position, // ref rayOrigin, ref rayDirection, out fraction, out normal)); //} }
/// <summary> /// Inverses the direction of a vector. /// </summary> /// <param name="value">The vector to inverse.</param> /// <param name="result">The negated vector.</param> public static void Negate(ref JVector value, out JVector result) { float num0 = -value.X; float num1 = -value.Y; result.X = num0; result.Y = num1; }
public static Vector3 ToXNAVector3(JVector vector) { return new Vector3(vector.X, vector.Y, 0.0f); }
public static JVector Normalize(JVector value) { JVector result; JVector.Normalize(ref value, out result); return result; }
void DrawBox(BoxShape box, JVector pos, float orientation, Color color) { List<JVector> poly = new List<JVector> { new JVector(-box.Size.X * 0.5f, -box.Size.Y * 0.5f), new JVector(box.Size.X * 0.5f, -box.Size.Y * 0.5f), new JVector(box.Size.X * 0.5f, box.Size.Y * 0.5f), new JVector(-box.Size.X * 0.5f,box.Size.Y * 0.5f) }; DrawPoly(poly, pos, JMatrix.CreateRotationZ(orientation), color); }
/// <summary> /// Normalizes the given vector. /// </summary> /// <param name="value">The vector which should be normalized.</param> /// <param name="result">A normalized vector.</param> public static void Normalize(ref JVector value, out JVector result) { float num2 = (value.X * value.X) + (value.Y * value.Y); float num = 1f / ((float)Math.Sqrt((double)num2)); result.X = value.X * num; result.Y = value.Y * num; }
protected override void Initialize() { DebugDrawer = new DebugDrawer(this); this.Components.Add(DebugDrawer); Camera = new Camera(this); this.Components.Add(Camera); A = new BoxShape(4, 1); PA = new JVector(-1, 2); VA = new JVector(0, 0); OA = MathHelper.ToRadians(0); B = new BoxShape(1, 4); PB = new JVector(-3, 0); VB = new JVector(0, 0); OB = 0; base.Initialize(); }
public static JVector Subtract(JVector value1, JVector value2) { JVector result; JVector.Subtract(ref value1, ref value2, out result); return result; }