public Triangle(Vector3 pos1, Vector3 pos2, Vector3 pos3) { Pos1 = pos1; Pos2 = pos2; Pos3 = pos3; Normal = VMath.Cross(Pos3 - Pos1, Pos2 - Pos1); }
public void ApplyImpulse(Vector2R impulse, Vector2R contactVector) { // velocity += invmass * impulse; //if (float.IsNaN(velocity.X)) System.Diagnostics.Debugger.Break(); angularVelocity += invinertia * (float)VMath.Cross(contactVector, impulse); //if (float.IsInfinity(angularVelocity)) System.Diagnostics.Debugger.Break(); }
public bool IsIntersectWithRay(Ray ray) { var cross1 = VMath.Cross(Pos2 - Pos1, ray.pos - Pos1); var dot1 = VMath.Dot(cross1, ray.vec); var cross2 = VMath.Cross(Pos3 - Pos2, ray.pos - Pos2); var dot2 = VMath.Dot(cross2, ray.vec); if (dot1 > 0 ^ dot2 > 0) { return(false); } var cross3 = VMath.Cross(Pos1 - Pos3, ray.pos - Pos3); var dot3 = VMath.Dot(cross3, ray.vec); return(!(dot2 > 0 ^ dot3 > 0)); }
public override void ComputeMass(float density) { //calculate centroid and moment of inertia Vector2R c = new Vector2R(0, 0); // centroid double area = 0; double I = 0; double k_inv3 = 1.0 / 3.0; for (int i1 = 0; i1 < vertexCount; i1++) { Vector2R p1 = vertices[i1]; int i2 = i1 + 1 < vertexCount ? i1 + 1 : 0; Vector2R p2 = vertices[i2]; double D = VMath.Cross(p1, p2); double triangleArea = 0.5 * D; area += triangleArea; //use area to weight the centroid average, not just the vertex position c += VMath.MultVectDouble(p1 + p2, triangleArea * k_inv3); // triangleArea * k_inv3 * (p1 + p2); double intx2 = p1.X * p1.X + p2.X * p1.X + p2.X * p2.X; double inty2 = p1.Y * p1.Y + p2.Y * p1.Y + p2.Y * p2.Y; I += (0.25 * k_inv3 * D) * (intx2 + inty2); } c = VMath.MultVectDouble(c, 1.0 / area); //translate verticies to centroid (make centroid (0,0) //for the polygon in model space) //Not really necessary but I like doing this anyway for (int i = 0; i < vertexCount; i++) { vertices[i] -= c; } body.mass = density * (float)area; body.inertia = (float)I * density; }
public void Initialize() { e = Math.Min(a.restitution, b.restitution); sf = Math.Sqrt(a.staticFriction * a.staticFriction); df = Math.Sqrt(a.dynamicFriction * a.dynamicFriction); for (int i = 0; i < contact_count; i++) { Vector2R ra = contacts[i] - a.pos; Vector2R rb = contacts[i] - b.pos; Vector2R crossprod_b = VMath.Cross(b.angularVelocity, rb); Vector2R crossprod_a = VMath.Cross(a.angularVelocity, ra); Vector2R rv = b.velocity + crossprod_b - a.velocity - crossprod_a; //Vector2 rv = b.velocity + VMath.Cross(b.angularVelocity, rb) - // a.velocity - VMath.Cross(a.angularVelocity, ra); //if (b.velocity.IsFucked()) System.Diagnostics.Debugger.Break(); //removelater //if (a.velocity.IsFucked()) System.Diagnostics.Debugger.Break(); //removelater //if (rv.Length() < (dt,gravity).LengthSquared() + EPSILON) e = 0.0f; } }
public void ApplyImpulse() { if (GMath.Equal(a.invmass + b.invmass, 0)) { InfinitMassCorrection(); return; } for (int i = 0; i < contact_count; i++) { //calcuate radii from COM to contact Vector2R ra = contacts[i] - a.pos; Vector2R rb = contacts[i] - b.pos; //relative velocity Vector2R rv = b.velocity + VMath.Cross(b.angularVelocity, rb) - a.velocity - VMath.Cross(a.angularVelocity, ra); //relative velocity along the normal double contactVel = Vector2R.Dot(rv, normal); //do not resolve if velocities are seperating if (contactVel > 0) { return; } double raCrossN = VMath.Cross(ra, normal); double rbCrossN = VMath.Cross(rb, normal); double invMassSum = a.invmass + b.invmass + (raCrossN * raCrossN) * a.invinertia + (rbCrossN * rbCrossN) * b.invinertia; //calculate impulse scalar double j = -(1.0 + e) * contactVel; j /= invMassSum; j /= (double)contact_count; //apply impulse Vector2R impulse = VMath.MultVectDouble(normal, j); // normal * j; a.ApplyImpulse(-impulse, ra); b.ApplyImpulse(impulse, rb); //friction impulse rv = b.velocity + VMath.Cross(b.angularVelocity, rb) - a.velocity - VMath.Cross(a.angularVelocity, ra); Vector2R t = rv - (normal * Vector2R.Dot(rv, normal)); //t.Normalize(); VMath.NormalizeSafe(ref t); //j tangent magnitude double jt = -Vector2R.Dot(rv, t); jt /= invMassSum; jt /= (double)contact_count; //don't apply tiny friction impulses if (GMath.Equal(jt, 0.0)) { return; } //coulumbs law Vector2R tangentImpulse; if (Math.Abs(jt) < j * sf) { tangentImpulse = VMath.MultVectDouble(t, df); // t * df; } else { tangentImpulse = VMath.MultVectDouble(t, -j * df); // t * -j * df } //apply friction impulse a.ApplyImpulse(-tangentImpulse, ra); b.ApplyImpulse(tangentImpulse, rb); } }
public void Set(Vector2R[] verts, int count) { //no hulls with less than 3 verticies (ensure actual polygon) //Debug.Assert(count > 2 && count < MaxPolyVertexCount); count = Math.Min(count, MaxPolyVertexCount); //find the right most point in the hull int rightMost = 0; double highestXCoord = verts[0].X; for (int i = 1; i < count; i++) { double x = verts[0].X; if (x > highestXCoord) { highestXCoord = x; rightMost = i; } //if matching x then take farthest negative y else if (x == highestXCoord && verts[i].Y < verts[rightMost].Y) { rightMost = i; } } int[] hull = new int[MaxPolyVertexCount]; int outCount = 0; int indexHull = rightMost; for (;;) { hull[outCount] = indexHull; // search for next index that wraps around the hull // by computing cross products to find the most counter-clockwise // vertex in the set, given the previous hull index int nextHullIndex = 0; for (int i = 1; i < count; i++) { //skip if same coordinate as we need three unique //points in the set to perform a cross product if (nextHullIndex == indexHull) { nextHullIndex = i; continue; } // cross every set of three unquie verticies // record each counter clockwise third vertex and add // to the output hull Vector2R e1 = verts[nextHullIndex] - verts[hull[outCount]]; Vector2R e2 = verts[i] - verts[hull[outCount]]; double c = VMath.Cross(e1, e2); if (c < 0.0f) { nextHullIndex = i; } // Cross product is zero then e vectors are on same line // therefor want to record vertex farthest along that line if (c == 0.0f && e2.LengthSquared() > e1.LengthSquared()) { nextHullIndex = i; } } outCount++; indexHull = nextHullIndex; //conclude algorithm upon wraparound if (nextHullIndex == rightMost) { vertexCount = outCount; break; } } float maxDist = 0; // Copy vertices into shape's vertices for (int i = 0; i < vertexCount; ++i) { vertices[i] = verts[hull[i]]; float dist = Vector2R.Distance(Vector2R.Zero, vertices[i]); if (dist > maxDist) { maxDist = dist; } } polyReach = maxDist * 2; ComputeNormals(); }