public static float CalcEffectiveMass(PBody b1, PBody b2, Vector2f r1, Vector2f r2, Vector2f normal) { float rn1 = normal.Dot(r1); float rn2 = normal.Dot(r2); return(1.0F / (b1.invM + b2.invM + b1.invI * ((r1.x * r1.x + r1.y * r1.y) - rn1 * rn1) + b2.invI * ((r2.x * r2.x + r2.y * r2.y) - rn2 * rn2))); }
public void CalculateCircumcircle(IList <Vector2f> vertices) { Vector2f p = vertices[i0]; Vector2f q = vertices[i1]; Vector2f r = vertices[i2]; // calculate the intersection of two perpendicular bisectors Vector2f pq = q - p; Vector2f qr = r - q; // check winding if (Vector2f.Cross(pq, qr) < 0.0f) { throw new InvalidOperationException("Triangle winding order incorrect"); } // mid-points of edges Vector2f a = 0.5f * (p + q); Vector2f b = 0.5f * (q + r); Vector2f u = pq.PerpendicularCCW; float d = Vector2f.Dot(u, qr); float t = Vector2f.Dot(b - a, qr) / d; CircumCenter = a + t * u; CircumRadius = (CircumCenter - p).Magnitude; }
public static Vector2f Projection(this Vector2f obj, Vector2f vector) { float vectorLen = vector.Len(); float len = obj.Dot(vector) / vectorLen; return(vector / vectorLen * len); }
public static List <VertexArray> GenerateLineWithThickness(List <Vector2f> points, Color color, float thickness, bool[] lines) { List <VertexArray> result = new List <VertexArray>( ); VertexArray array = new VertexArray(PrimitiveType.TrianglesStrip); for (int i = 0; i < points.Count; i++) { Vector2f v0 = (i == 0 ? 2 * points[0] - points[1] : points[i - 1]); Vector2f v1 = points[i]; Vector2f v2 = (i == points.Count - 1 ? 2 * points[i] - points[i - 1] : points[i + 1]); Vector2f v01 = (v1 - v0).Normalized( ); Vector2f v12 = (v2 - v1).Normalized( ); Vector2f d = (v01 + v12).GetNormal( ); float dot = d.Dot(v01.GetNormal( )); d *= thickness / 2f / dot; //< TODO: Add flat miter joint in extreme cases if (lines[i % lines.Length]) { array.Append(new Vertex(v1 + d, color)); array.Append(new Vertex(v1 - d, color)); } else { if (array.VertexCount > 0) { result.Add(array); array = new VertexArray(PrimitiveType.TrianglesStrip); } } } if (array.VertexCount > 0) { result.Add(array); } return(result); }
private void CollidePlanes() { for (int i = 0; i < Particles.Count; ++i) { for (int p = 0; p < Planes.Count; ++p) { Vector2f n = Planes[p].xy; float d = Vector2f.Dot(Particles[i].p, n) + Planes[p].z; if (d < 0.0f) { // push out of halfspace Particles[i].p -= d * n; // make relative velocity separating float rv = Vector2f.Dot(Particles[i].v, n); if (rv < 0.0f) { // zero normal velocity, material simulation will take care of restitution Vector2f nv = -rv * n; // friction Vector2f tv = (Particles[i].v + nv) * Friction; // update velocity Particles[i].v = tv; } } } } }
VertexArray GenerateTrianglesStrip(List <Vector2f> points, Color color, float thickness, bool open) { var array = new VertexArray(PrimitiveType.TrianglesStrip); for (int i = 1; i < points.Count + 1 + (open ? 0 : 1); i++) { Vector2f v0 = points[(i - 1) % points.Count]; Vector2f v1 = points[i % points.Count]; Vector2f v2 = points[(i + 1) % points.Count]; Vector2f v01 = (v1 - v0).Normalized(); Vector2f v12 = (v2 - v1).Normalized(); Vector2f d = (v01 + v12).GetNormal(); float dot = d.Dot(v01.GetNormal()); d *= thickness / 2f / dot; //< TODO: Add flat miter joint in extreme cases // d *= thickness / 2f / (float)Math.Max(.8, dot); if (points.Count == i) { array.Append(new Vertex(v0 - d / 4, color)); array.Append(new Vertex(v0 + d / 4, color)); } else { array.Append(new Vertex(v0 + d, color)); array.Append(new Vertex(v0 - d, color)); } } return(array); }
private PPolygonPolygonCollider.PWContactedVertex [] GetEdgeOfPotentialCollision( PConvexPolygonShape p1, PConvexPolygonShape p2, int r1edge, bool flip) { PPolygonPolygonCollider.PWContactedVertex [] line = new PPolygonPolygonCollider.PWContactedVertex [2]; Vector2f normal = p1.nors[r1edge]; float dist = 1.0F; int ver = -1; int nextVer = -1; for (int i = 0; i < p2.numVertices; i++) { float dot = normal.Dot(p2.nors[i]); if (dot < dist || dist == 1.0F) { dist = dot; ver = i; nextVer = (i + 1) % p2.numVertices; } } line[0] = new PPolygonPolygonCollider.PWContactedVertex(); line[0].v.Set(p2.vers[ver].x, p2.vers[ver].y); line[0].data.Set(r1edge + ver * 2 + ver * 4, false); line[1] = new PPolygonPolygonCollider.PWContactedVertex(); line[1].v.Set(p2.vers[nextVer].x, p2.vers[nextVer].y); line[1].data.Set(r1edge + ver * 2 + nextVer * 4, false); return(line); }
public Projection(Vector2f axis, List<Vector2f> vertices) { this.axis = axis; min = axis.Dot(vertices[0]); max = min; for(int i = 1; i < vertices.Count; ++i) { float p = axis.Dot(vertices[i]); if(p < min) min = p; else if(p > max) max = p; } }
/// <summary> /// If the segment ab intersects the curve. /// </summary> public bool Intersects(Vector2f a, Vector2f b) { //coefficients of quadratic Vector2f c2 = C0 + C1 * -2.0f + C2; Vector2f c1 = C0 * -2.0f + C1 * 2.0f; //Convert line to normal form: ax + by + c = 0 //Find normal to line: negative inverse of original line's slope Vector2f n = new Vector2f(a.y - b.y, b.x - a.x); //c coefficient for normal form of line float c = a.x * b.y - b.x * a.y; //Transform coefficients to line's coordinate system and find roots of cubic var roots = Polynomial3d.Solve(1, Vector2f.Dot(n, c2), Vector2f.Dot(n, c1), Vector2f.Dot(n, C0) + c); Vector2f min, max; min.x = Math.Min(a.x, b.x); min.y = Math.Min(a.y, b.y); max.x = Math.Max(a.x, b.x); max.y = Math.Max(a.y, b.y); for (int i = 0; i < roots.real; i++) { float t = (float)roots[i]; if (t < 0.0f || t > 1.0f) { continue; } Vector2f v0 = Position(t); if (a.x == b.x) { if (min.y <= v0.y && v0.y <= max.y) { return(true); } } else if (a.y == b.y) { if (min.x <= v0.x && v0.x <= max.x) { return(true); } } else if (min.x <= v0.x && v0.x <= max.x && min.y <= v0.y && v0.y <= max.y) { return(true); } } return(false); }
internal override void SolvePosition() { float rvn = normal.Dot(PTransformer.CalcRelativeCorrectVelocity(b1, b2, relAnchor1, relAnchor2)); float impulse = -mass * ((dist - length) + rvn) * 0.2F; if (impulse > 0.0F) { impulse = Max(impulse - 0.002F, 0.0F); } else if (impulse < 0.0F) { impulse = Min(impulse + 0.002F, 0.0F); } float forceX = normal.x * impulse; float forceY = normal.y * impulse; b1.PositionCorrection(forceX, forceY, anchor1.x, anchor1.y); b2.PositionCorrection(-forceX, -forceY, anchor2.x, anchor2.y); }
private PPolygonPolygonCollider.PWContactedVertex [] ClipEdge(PPolygonPolygonCollider.PWContactedVertex [] clips, Vector2f normal, float dist) { PPolygonPolygonCollider.PWContactedVertex [] line = new PPolygonPolygonCollider.PWContactedVertex [2]; int numClips = 0; float dist0 = normal.Dot(clips[0].v) - dist; float dist1 = normal.Dot(clips[1].v) - dist; if (dist0 < 0.0F) { line[numClips] = clips[0]; numClips++; } if (dist1 < 0.0F) { line[numClips] = clips[1]; numClips++; } if (numClips == 0) { return(null); } if (numClips == 2) { return(line); } int c = 0; if (dist0 < 0.0F && dist1 > 0.0F) { c = 1; } float d = dist0 / (dist0 - dist1); line[1] = new PPolygonPolygonCollider.PWContactedVertex(); line[1].v = clips[1].v.Sub(clips[0].v).Clone(); line[1].v.MulLocal(d); line[1].v.AddLocal(clips[0].v); line[1].data = clips[c].data; return(line); }
void MoveByMouseDrag(float dt) { float dampStrength = 10; if (MouseIndex != -1) { Vector2f pq = MousePos - Scene.Particles[MouseIndex].p; Vector2f damp = -dampStrength *Vector2f.Dot(pq.Normalized, Scene.Particles[MouseIndex].v) * pq.Normalized; Vector2f stretch = mouseStrength * pq; Scene.Particles[MouseIndex].f += stretch + damp; } }
protected bool IsLightedByPlayer() { if (world.avatar.body.position.DistanceTo(entity.body.position) < LIGHTABLE_DISTANCE) { float avatarLookAngle = world.avatar.sprite.rotation; Vector2f avatarLook = new Vector2f( Mathf.Cos(avatarLookAngle * Mathf.DEG2RAD), Mathf.Sin(avatarLookAngle * Mathf.DEG2RAD) ); Vector2f myLook = (world.avatar.body.position - entity.body.position).Normalized(); float dot = myLook.Dot(avatarLook); return(dot < -0.5f); } else { return(false); } }
public static float SqrDistanceFromSegment(Segment2f seg, Vector2f p) { Vector2f ab = seg.B - seg.A; Vector2f ac = p - seg.A; Vector2f bc = p - seg.B; float e = Vector2f.Dot(ac, ab); // Handle cases where c projects outside ab if (e <= 0.0) { return(Vector2f.Dot(ac, ac)); } float f = Vector2f.Dot(ab, ab); if (e >= f) { return(Vector2f.Dot(bc, bc)); } // Handle case where p projects onto ab return(Vector2f.Dot(ac, ac) - e * e / f); }
/// <summary> /// Gets the intersection between the convex shape and the ray. /// </summary> /// <param name="ray">Ray to test.</param> /// <param name="transform">Transform of the convex shape.</param> /// <param name="maximumLength">Maximum distance to travel in units of the ray direction's length.</param> /// <param name="hit">Ray hit data, if any.</param> /// <returns>Whether or not the ray hit the target.</returns> public override bool RayTest(ref Ray ray, ref RigidTransform transform, float maximumLength, out RayHit hit) { //Put the ray into local space. Quaternion conjugate; Quaternion.Conjugate(ref transform.Orientation, out conjugate); Ray localRay; Vector3f.Subtract(ref ray.Position, ref transform.Position, out localRay.Position); Vector3f.Transform(ref localRay.Position, ref conjugate, out localRay.Position); Vector3f.Transform(ref ray.Direction, ref conjugate, out localRay.Direction); //Check for containment in the cylindrical portion of the capsule. if (localRay.Position.Y >= -halfLength && localRay.Position.Y <= halfLength && localRay.Position.X * localRay.Position.X + localRay.Position.Z * localRay.Position.Z <= collisionMargin * collisionMargin) { //It's inside! hit.T = 0; hit.Location = localRay.Position; hit.Normal = new Vector3f(hit.Location.X, 0, hit.Location.Z); float normalLengthSquared = hit.Normal.LengthSquared; if (normalLengthSquared > 1e-9f) { Vector3f.Divide(ref hit.Normal, (float)Math.Sqrt(normalLengthSquared), out hit.Normal); } else { hit.Normal = new Vector3f(); } //Pull the hit into world space. Vector3f.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal); RigidTransform.Transform(ref hit.Location, ref transform, out hit.Location); return(true); } //Project the ray direction onto the plane where the cylinder is a circle. //The projected ray is then tested against the circle to compute the time of impact. //That time of impact is used to compute the 3d hit location. Vector2f planeDirection = new Vector2f(localRay.Direction.X, localRay.Direction.Z); float planeDirectionLengthSquared = planeDirection.LengthSquared; if (planeDirectionLengthSquared < MathHelper.Epsilon) { //The ray is nearly parallel with the axis. //Skip the cylinder-sides test. We're either inside the cylinder and won't hit the sides, or we're outside //and won't hit the sides. if (localRay.Position.Y > halfLength) { goto upperSphereTest; } if (localRay.Position.Y < -halfLength) { goto lowerSphereTest; } hit = new RayHit(); return(false); } Vector2f planeOrigin = new Vector2f(localRay.Position.X, localRay.Position.Z); float dot; Vector2f.Dot(ref planeDirection, ref planeOrigin, out dot); float closestToCenterT = -dot / planeDirectionLengthSquared; Vector2f closestPoint; Vector2f.Multiply(ref planeDirection, closestToCenterT, out closestPoint); Vector2f.Add(ref planeOrigin, ref closestPoint, out closestPoint); //How close does the ray come to the circle? float squaredDistance = closestPoint.LengthSquared; if (squaredDistance > collisionMargin * collisionMargin) { //It's too far! The ray cannot possibly hit the capsule. hit = new RayHit(); return(false); } //With the squared distance, compute the distance backward along the ray from the closest point on the ray to the axis. float backwardsDistance = collisionMargin * (float)Math.Sqrt(1 - squaredDistance / (collisionMargin * collisionMargin)); float tOffset = backwardsDistance / (float)Math.Sqrt(planeDirectionLengthSquared); hit.T = closestToCenterT - tOffset; //Compute the impact point on the infinite cylinder in 3d local space. Vector3f.Multiply(ref localRay.Direction, hit.T, out hit.Location); Vector3f.Add(ref hit.Location, ref localRay.Position, out hit.Location); //Is it intersecting the cylindrical portion of the capsule? if (hit.Location.Y <= halfLength && hit.Location.Y >= -halfLength && hit.T < maximumLength) { //Yup! hit.Normal = new Vector3f(hit.Location.X, 0, hit.Location.Z); float normalLengthSquared = hit.Normal.LengthSquared; if (normalLengthSquared > 1e-9f) { Vector3f.Divide(ref hit.Normal, (float)Math.Sqrt(normalLengthSquared), out hit.Normal); } else { hit.Normal = new Vector3f(); } //Pull the hit into world space. Vector3f.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal); RigidTransform.Transform(ref hit.Location, ref transform, out hit.Location); return(true); } if (hit.Location.Y < halfLength) { goto lowerSphereTest; } upperSphereTest: //Nope! It may be intersecting the ends of the capsule though. //We're above the capsule, so cast a ray against the upper sphere. //We don't have to worry about it hitting the bottom of the sphere since it would have hit the cylinder portion first. var spherePosition = new Vector3f(0, halfLength, 0); if (Toolbox.RayCastSphere(ref localRay, ref spherePosition, collisionMargin, maximumLength, out hit)) { //Pull the hit into world space. Vector3f.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal); RigidTransform.Transform(ref hit.Location, ref transform, out hit.Location); return(true); } //No intersection! We can't be hitting the other sphere, so it's over! hit = new RayHit(); return(false); lowerSphereTest: //Okay, what about the bottom sphere? //We're above the capsule, so cast a ray against the upper sphere. //We don't have to worry about it hitting the bottom of the sphere since it would have hit the cylinder portion first. spherePosition = new Vector3f(0, -halfLength, 0); if (Toolbox.RayCastSphere(ref localRay, ref spherePosition, collisionMargin, maximumLength, out hit)) { //Pull the hit into world space. Vector3f.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal); RigidTransform.Transform(ref hit.Location, ref transform, out hit.Location); return(true); } //No intersection! We can't be hitting the other sphere, so it's over! hit = new RayHit(); return(false); }
private bool Solve(Game0 game, DynamicEntity entity, out Vector2f position, out Vector2f velocity) { position = entity.Position; velocity = entity.Velocity; // limit falling velocity velocity.Y = Math.Max(velocity.Y, TerminalFallingVelocity); // no velocity ? no collision! if (velocity == Vector2f.Zero) { return(false); } var obstacles = game.Entities.Where(e => (e as Obstacle) != null).Select(o => o as Obstacle).ToArray(); var remainingVelocity = velocity; var collisionFound = false; while (true) { var distance = velocity.Length(); var direction = velocity.Normalize(); var entityBox = entity.BoundingBox + position; var t = Collisions.Trace(entityBox, velocity, obstacles.Select(o => o.BoundingBox + o.Position).ToArray(), out Vector2f normal); if (t.LeEq(distance)) { collisionFound = true; var hitPosition = position + direction * t; // set new position & velocity velocity = (position + velocity) + normal * Vector2f.Dot(direction.Inv(), normal) * (distance - t) - hitPosition; position = hitPosition; if (normal.X == 0) { remainingVelocity.Y = 0; } else { remainingVelocity.X = 0; } if (velocity == Vector2f.Zero) // or better just close to zero ? { break; // solved - nowhere to move } } else { break; // solved - nothing else stands in the way } } position += velocity; velocity = remainingVelocity; return(collisionFound); }
private void UpdateForces(float dt, bool performFracture) { for (int i = 0; i < Particles.Count; ++i) { Particles[i].max = 0; if (Particles[i].invMass > 0.0f) { Particles[i].f += Gravity / Particles[i].invMass; } else { Particles[i].f += Vector2f.Zero - Drag * Particles[i].v; } } Vector2f[] x = new Vector2f[3]; Vector2f[] v = new Vector2f[3]; for (int i = 0; i < Triangles.Count; ++i) { Triangle tri = Triangles[i]; FEMElement elem = Elements[i]; x[0] = Particles[tri.i].p; x[1] = Particles[tri.j].p; x[2] = Particles[tri.k].p; v[0] = Particles[tri.i].v; v[1] = Particles[tri.j].v; v[2] = Particles[tri.k].v; if (performFracture) { Matrix2x2f f = CalcDeformation(x, elem.mInvDm); Matrix2x2f q = Decomposition2x2f.QRDecomposition(f); // strain Matrix2x2f e = CalcCauchyStrainTensor(q.Transpose * f); // update plastic strain float ef = FrobeniusNorm(e); if (ef > Yield) { elem.mEp += e * dt * Creep; } const float epmax = 0.6f; if (ef > epmax) { elem.mEp *= epmax / ef; } // adjust strain e -= elem.mEp; Matrix2x2f s = CalcStressTensor(e, LameLambda, LameMu); // damping forces Matrix2x2f dfdt = CalcDeformation(v, elem.mInvDm); Matrix2x2f dedt = CalcCauchyStrainTensorDt(q.Transpose * dfdt); Matrix2x2f dsdt = CalcStressTensor(dedt, Damping, Damping); Matrix2x2f p = s + dsdt; float e1, e2; Decomposition2x2f.EigenDecomposition(p, out e1, out e2); float me = Mathf.Max(e1, e2); if (me > Toughness) { // calculate Eigenvector corresponding to max Eigenvalue Vector2f ev = q * (new Vector2f(p.m01, me - p.m00)).Normalized; // pick a random vertex to split on int splitNode = Rnd.Next(0, 2); // don't fracture immovable nodes if (Particles[GetVertex(tri, splitNode)].invMass == 0.0f) { break; } // fracture plane perpendicular to ev Vector3f plane = new Vector3f(ev.x, ev.y, -Vector2f.Dot(ev, Particles[GetVertex(tri, splitNode)].p)); FEMFractureEvent fracture = new FEMFractureEvent(); fracture.Tri = i; fracture.Node = splitNode; fracture.Plane = plane; //Fracture not implemented so these fracture planes are not used. Fractures.Add(fracture); } // calculate force on each edge due to stress and distribute to the nodes Vector2f f1 = q * p * elem.mB[0]; Vector2f f2 = q * p * elem.mB[1]; Vector2f f3 = q * p * elem.mB[2]; Particles[tri.i].f -= f1 / 3.0f; Particles[tri.j].f -= f2 / 3.0f; Particles[tri.k].f -= f3 / 3.0f; } else { //This was the code used when fracturing was disabled //in the original. It seems very unstable for me. maybe //a bug or precision issue. Not used atm. Matrix2x2f f = CalcDeformation(x, elem.mInvDm); // elastic forces Matrix2x2f e = CalcGreenStrainTensor(f); Matrix2x2f s = CalcStressTensor(e, LameLambda, LameMu); // damping forces Matrix2x2f dfdt = CalcDeformation(v, elem.mInvDm); Matrix2x2f dedt = CalcGreenStrainTensorDt(f, dfdt); Matrix2x2f dsdt = CalcStressTensor(dedt, Damping, Damping); Matrix2x2f p = s + dsdt; float e1, e2; Decomposition2x2f.EigenDecomposition(p, out e1, out e2); float me = Mathf.Max(e1, e2); Matrix2x2f finv = f.Transpose.Inverse; Vector2f f1 = p * (finv * elem.mB[0]); Vector2f f2 = p * (finv * elem.mB[1]); Vector2f f3 = p * (finv * elem.mB[2]); Particles[tri.i].f -= f1 / 3.0f; Particles[tri.j].f -= f2 / 3.0f; Particles[tri.k].f -= f3 / 3.0f; Particles[tri.i].max += me / 3.0f; Particles[tri.j].max += me / 3.0f; Particles[tri.k].max += me / 3.0f; } } }
private bool IsPointInsideDomain(Vector2f coordinate) { bool result = false; Vector2f origin = new Vector2f(-100000, -100000); float minDist = int.MaxValue; int nbIntersect = 0; for (int i = 0; i < this.domainPoints.Count; i++) { Vector2f point1 = this.domainPoints[i]; Vector2f point2; if (i == this.domainPoints.Count - 1) { point2 = this.domainPoints[0]; } else { point2 = this.domainPoints[i + 1]; } float num1 = point1.X * point2.Y - point1.Y * point2.X; float num2 = origin.X * coordinate.Y - origin.Y * coordinate.X; float denum = (point1.X - point2.X) * (origin.Y - coordinate.Y) - (point1.Y - point2.Y) * (origin.X - coordinate.X); if (denum != 0) { float intersecX = (num1 * (origin.X - coordinate.X) - num2 * (point1.X - point2.X)) / denum; float intersecY = (num1 * (origin.Y - coordinate.Y) - num2 * (point1.Y - point2.Y)) / denum; Vector2f intersect = new Vector2f(intersecX, intersecY); if (intersect != point2 && (intersect - point1).Dot(intersect - point2) < 0 && (intersect - origin).Dot(intersect - coordinate) < 0) { nbIntersect++; } } Vector2f firstVector = coordinate - point1; Vector2f normalizedEdge = (point2 - point1).Normalize(); //vec2 vector = vector - normalizedEdge * dot(normalizedEdge, vector); //vec3 crossVector = cross(vec3(firstVector, 0), vec3(normalizedEdge, 0)); //vec3 crossVector2 = cross(vec3(vector2, 0), vec3(-normalizedEdge, 0)); Vector2f secondVector = coordinate - point2; if (normalizedEdge.Dot(firstVector) * normalizedEdge.Dot(secondVector) < 0) { float crossLen = Math.Abs(firstVector.CrossZ(normalizedEdge)); if (crossLen < minDist) { minDist = crossLen; } } float lenToPoint = firstVector.Len(); if (lenToPoint < minDist) { minDist = lenToPoint; } } if (this.isFilled) { if (nbIntersect % 2 == 1) { result = true; } } if (minDist < MARGIN_DOMAIN / 2) { result = true; } return(result); }
public static Vector2f ClosestPointOnTriangle(Triangle2f triangle, Vector2f p) { Vector2f ab = triangle.B - triangle.A; Vector2f ac = triangle.C - triangle.A; Vector2f ap = p - triangle.A; // Check if P in vertex region outside A float d1 = Vector2f.Dot(ab, ap); float d2 = Vector2f.Dot(ac, ap); if (d1 <= 0.0 && d2 <= 0.0) { // barycentric coordinates (1,0,0) return(triangle.A); } float v, w; // Check if P in vertex region outside B Vector2f bp = p - triangle.B; float d3 = Vector2f.Dot(ab, bp); float d4 = Vector2f.Dot(ac, bp); if (d3 >= 0.0 && d4 <= d3) { // barycentric coordinates (0,1,0) return(triangle.B); } // Check if P in edge region of AB, if so return projection of P onto AB float vc = d1 * d4 - d3 * d2; if (vc <= 0.0 && d1 >= 0.0f && d3 <= 0.0) { v = d1 / (d1 - d3); // barycentric coordinates (1-v,v,0) return(triangle.A + v * ab); } // Check if P in vertex region outside C Vector2f cp = p - triangle.C; float d5 = Vector2f.Dot(ab, cp); float d6 = Vector2f.Dot(ac, cp); if (d6 >= 0.0 && d5 <= d6) { // barycentric coordinates (0,0,1) return(triangle.C); } // Check if P in edge region of AC, if so return projection of P onto AC float vb = d5 * d2 - d1 * d6; if (vb <= 0.0 && d2 >= 0.0 && d6 <= 0.0) { w = d2 / (d2 - d6); // barycentric coordinates (1-w,0,w) return(triangle.A + w * ac); } // Check if P in edge region of BC, if so return projection of P onto BC float va = d3 * d6 - d5 * d4; if (va <= 0.0 && (d4 - d3) >= 0.0 && (d5 - d6) >= 0.0) { w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); // barycentric coordinates (0,1-w,w) return(triangle.B + w * (triangle.C - triangle.B)); } // P inside face region. Compute Q through its barycentric coordinates (u,v,w) float denom = 1.0f / (va + vb + vc); v = vb * denom; w = vc * denom; // = u*a + v*b + w*c, u = va * denom = 1.0f - v - w return(triangle.A + ab * v + ac * w); }
public static float Angle(Vector2f a, Vector2f b) { return(Math.Acos(Math.Clamp(Vector2f.Dot(Normalize(a), Normalize(b)), -1f, 1f))); }
// apply a force at 'fromCenter' relative to center of the shape public void ApplyTorque(Vector2f force, Vector2f fromCenter) { var radius = fromCenter.Magnitude(); // check if 'fromCenter' is outside of the shape. if(radius > GetRadiusOn(fromCenter)) return; // torque is the amount of force in the perpendicular direction var torque = force.Cross(fromCenter); // linear force is the amount of force in the parallel direction var linearForce = force.Dot(fromCenter) / (radius != 0 ? radius : 1f); // moment of inertia of the mass at this radius var inertia = mass * radius * radius; AngularAcceleration += -torque / (inertia != 0 ? inertia : 1); ApplyForce(linearForce * force.Unit()); }
public void Dot() { Assert.AreEqual(Vector2f.Dot(Vector2f.One, Vector2f.One), 2.0f); }
public static void ClosestSegmentToSegments(Segment2f seg0, Segment2f seg1, out float s, out float t) { Vector2f ab0 = seg0.B - seg0.A; Vector2f ab1 = seg1.B - seg1.A; Vector2f a01 = seg0.A - seg1.A; float d00 = Vector2f.Dot(ab0, ab0); float d11 = Vector2f.Dot(ab1, ab1); float d1 = Vector2f.Dot(ab1, a01); s = 0; t = 0; //Check if either or both segments degenerate into points. if (d00 < FMath.EPS && d11 < FMath.EPS) { return; } if (d00 < FMath.EPS) { //First segment degenerates into a point. s = 0; t = FMath.Clamp01(d1 / d11); } else { float c = Vector2f.Dot(ab0, a01); if (d11 < FMath.EPS) { //Second segment degenerates into a point. s = FMath.Clamp01(-c / d00); t = 0; } else { //The generate non degenerate case starts here. float d2 = Vector2f.Dot(ab0, ab1); float denom = d00 * d11 - d2 * d2; //if segments not parallel compute closest point and clamp to segment. if (!FMath.IsZero(denom)) { s = FMath.Clamp01((d2 * d1 - c * d11) / denom); } else { s = 0; } t = (d2 * s + d1) / d11; if (t < 0.0f) { t = 0.0f; s = FMath.Clamp01(-c / d00); } else if (t > 1.0f) { t = 1.0f; s = FMath.Clamp01((d2 - c) / d00); } } } }
public VertexArray GetVisMesh() { if (_boundsNeedUpdate) { ComputeBoundaries(); _boundsNeedUpdate = false; } // if segments state is clean, no need to calculate mesh again if (!_visMeshNeedsUpdate) return _visMesh; // add all segments to a temp list _visibleSegments = new List<Segment>(_segments); // get all unique VISIBLE segments' endpoints + bound endpoints var points = new HashSet<Vector2f>(); for (var i = _visibleSegments.Count - 1; i >= 0; i--) { var s = _visibleSegments[i]; var edge = s.Start - s.End; var normal = new Vector2f(edge.Y, -edge.X); // is the segment visible from the ray origin? var distVector = s.End - _center; var dot = normal.Dot(distVector); if (dot > 0 && distVector.Length() <= _radius) { points.Add(s.Start); points.Add(s.End); } else { _visibleSegments.RemoveAt(i); } } // add bounding points foreach (var s in _bounds) { points.Add(s.Start); points.Add(s.End); } // add bounding segments to visible segments _visibleSegments.AddRange(_bounds); // get all angles var angles = new List<float>(); foreach ( var angle in points.Select(p => Math.Atan2(p.Y - _center.Y, p.X - _center.X)).Select(angle => (float) angle)) { angles.Add(angle - 0.0001f); angles.Add(angle); angles.Add(angle + 0.0001f); } // rays to all visible endpoints var intersections = new List<Ray>(); foreach (var angle in angles) { var dx = Math.Cos(angle); var dy = Math.Sin(angle); var rayStart = _center; var rayEnd = new Vector2f(_center.X + (float) dx, _center.Y + (float) dy); // find closest intersection Ray closestIntersect = null; foreach ( var intersect in _visibleSegments .Select(s => MathUtils.GetIntersection(rayStart, rayEnd, s.Start, s.End)) .Where(intersect => intersect != null) .Where(intersect => closestIntersect == null || intersect.Length < closestIntersect.Length)) closestIntersect = intersect; if (closestIntersect == null) continue; closestIntersect.Angle = angle; intersections.Add(closestIntersect); } // sort intersects by angle intersections.Sort((a, b) => a.Angle.CompareTo(b.Angle)); // make visibility mesh _visMesh.Clear(); _visMesh.Append(new Vertex(_center, _center)); foreach (var hit in intersections) _visMesh.Append(new Vertex(hit.Position, hit.Position)); _visMesh.Append(new Vertex(intersections[0].Position, intersections[0].Position)); _visMeshNeedsUpdate = false; Console.WriteLine("mesh updated with " + intersections.Count + " hits"); return _visMesh; }
public virtual int Collide(PShape s1, PShape s2, PContact[] cs) { if (s1._type != PShapeType.CONVEX_SHAPE && s1._type != PShapeType.BOX_SHAPE || s2._type != PShapeType.CONVEX_SHAPE && s2._type != PShapeType.BOX_SHAPE) { return(0); } PConvexPolygonShape p1 = (PConvexPolygonShape)s1; PConvexPolygonShape p2 = (PConvexPolygonShape)s2; PPolygonPolygonCollider.PWDistanceData dis1 = GetDistance(p1, p2); if (dis1.dist > 0.0F) { return(0); } PPolygonPolygonCollider.PWDistanceData dis2 = GetDistance(p2, p1); if (dis2.dist > 0.0F) { return(0); } float error = 0.008F; int edgeA; PConvexPolygonShape pa; PConvexPolygonShape pb; bool flip; if (dis1.dist > dis2.dist + error) { pa = p1; pb = p2; edgeA = dis1.edge; flip = false; } else { pa = p2; pb = p1; edgeA = dis2.edge; flip = true; } Vector2f normal = pa.nors[edgeA]; Vector2f tangent = new Vector2f(-normal.y, normal.x); Vector2f[] paVers = pa.vers; PPolygonPolygonCollider.PWContactedVertex [] cv = GetEdgeOfPotentialCollision(pa, pb, edgeA, flip); cv = ClipEdge(cv, tangent.Negate(), -tangent.Dot(paVers[edgeA])); if (cv == null) { return(0); } cv = ClipEdge(cv, tangent, tangent.Dot(paVers[(edgeA + 1) % pa.numVertices])); if (cv == null) { return(0); } Vector2f contactNormal = (flip) ? normal : normal.Negate(); int numContacts = 0; for (int i = 0; i < 2; i++) { float dist = normal.Dot(cv[i].v) - normal.Dot(paVers[edgeA]); if (dist < 0.0F) { PContact c = new PContact(); c.normal.Set(contactNormal.x, contactNormal.y); c.pos.Set(cv[i].v.x, cv[i].v.y); c.overlap = dist; c.data = cv[i].data; c.data.flip = flip; cs[numContacts] = c; numContacts++; } } return(numContacts); }
public static float Dot(Vector2f a, Vector2f b) { return(Vector2f.Dot(a, b)); }