public void Cross()
{
Vector2f v01 = new Vector2f(0, 1);
Vector2f v10 = new Vector2f(1, 0);
Assert.AreEqual(Vector2f.Cross(v01, v10), -1.0f);
}
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 float TriArea(IList <Vector2f> vertices)
{
Vector2f a = vertices[i0];
Vector2f b = vertices[i1];
Vector2f c = vertices[i2];
return(0.5f * (Vector2f.Cross(b - a, c - a)));
}
public static bool TriangleContainsPointCCW(Triangle2f triangle, Vector2f p)
{
if (Vector2f.Cross(p - triangle.A, triangle.B - triangle.A) > 0.0)
{
return(false);
}
if (Vector2f.Cross(p - triangle.B, triangle.C - triangle.B) > 0.0)
{
return(false);
}
if (Vector2f.Cross(p - triangle.C, triangle.A - triangle.C) > 0.0)
{
return(false);
}
return(true);
}
public static bool TriangleContainsPoint(Triangle2f triangle, Vector2f p)
{
float pab = Vector2f.Cross(p - triangle.A, triangle.B - triangle.A);
float pbc = Vector2f.Cross(p - triangle.B, triangle.C - triangle.B);
if (Math.Sign(pab) != Math.Sign(pbc))
{
return(false);
}
float pca = Vector2f.Cross(p - triangle.C, triangle.A - triangle.C);
if (Math.Sign(pab) != Math.Sign(pca))
{
return(false);
}
return(true);
}
// 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 static float Cross(Vector2f a, Vector2f b)
{
return(Vector2f.Cross(a, b));
}
public PConvexPolygonShape(float[] xvers, float[] yvers, float density)
{
float[] xs = (float[])xvers.Clone();
float[] ys = (float[])yvers.Clone();
numVertices = xs.Length;
_dens = density;
localVers = new Vector2f[numVertices];
nors = new Vector2f[numVertices];
float fakeCenterY;
float fakeCenterX = fakeCenterY = 0.0F;
for (int i = 0; i < numVertices; i++)
{
fakeCenterX += xs[i];
fakeCenterY += ys[i];
}
fakeCenterX /= numVertices;
fakeCenterY /= numVertices;
for (int i = 0; i < numVertices; i++)
{
localVers[i] = new Vector2f(xs[i] - fakeCenterX, ys[i]
- fakeCenterY);
nors[i] = new Vector2f();
}
vers = new Vector2f[numVertices];
for (int j = 0; j < localVers.Length; j++)
{
mm += localVers[j].Cross(localVers[(j + 1) % numVertices]) * 0.5F;
}
float cy;
float cx = cy = 0.0F;
float invThree = 0.3333333F;
for (int j = 0; j < localVers.Length; j++)
{
Vector2f ver = localVers[j];
Vector2f nextVer = localVers[(j + 1) % numVertices];
float triArea = ver.Cross(nextVer) * 0.5F;
cx += triArea * (ver.x + nextVer.x) * invThree;
cy += triArea * (ver.y + nextVer.y) * invThree;
}
float invM = 1.0F / mm;
cx *= invM;
cy *= invM;
cx += fakeCenterX;
cy += fakeCenterY;
for (int i = 0; i < numVertices; i++)
{
localVers[i].x += fakeCenterX;
localVers[i].y += fakeCenterY;
}
_localPos.Set(cx, cy);
for (int i = 0; i < numVertices; i++)
{
vers[i] = new Vector2f(localVers[i].x, localVers[i].y);
}
for (int i = 0; i < numVertices; i++)
{
localVers[i].SubLocal(_localPos);
}
float invSix = 0.1666667F;
for (int j = 0; j < localVers.Length; j++)
{
Vector2f ver = localVers[j];
Vector2f nextVer = localVers[(j + 1) % numVertices];
float triArea = ver.Cross(nextVer) * 0.5F;
this.ii += triArea
* invSix
* (ver.x * ver.x + ver.y * ver.y + ver.x * nextVer.x
+ ver.y * nextVer.y + nextVer.x * nextVer.x + nextVer.y
* nextVer.y);
}
localNors = new Vector2f[numVertices];
for (int i = 0; i < numVertices; i++)
{
Vector2f ver = localVers[i];
Vector2f nextVer = localVers[(i + 1) % localVers.Length];
localNors[i] = new Vector2f(nextVer.y - ver.y, -nextVer.x + ver.x);
localNors[i].Normalize();
}
_type = PShapeType.CONVEX_SHAPE;
SetDensity(_dens);
CalcAABB();
}
public static FEMScene Create(Texture2D img)
{
FEMScene Scene = new FEMScene();
if (img == null)
{
return(Scene);
}
Scene.Substeps = 80;
Scene.Drag = 0.1f;
Scene.LameLambda = 42000.0f;
Scene.LameMu = 42000.0f;
Scene.Damping = 250.0f;
Scene.Friction = 0.8f;
Scene.Toughness = 40000.0f;
List <Vector2f> points = new List <Vector2f>();
List <Vector2f> bpoints = new List <Vector2f>();
// controls how finely the object is sampled
aspect = (float)img.height / img.width;
float fw = img.width;
float fh = img.height;
int inc = Mathf.FloorToInt(img.width * resolution);
int margin = Mathf.Max((inc - 1) / 2, 1);
// distribute points interior to the object or near it's boundary
for (int y = 0; y < img.height; y += inc)
{
for (int x = 0; x < img.width; x += inc)
{
// if value non-zero then add a point
int n = Neighbours(img, x, y, margin);
if (n > 0)
{
Vector2f uv = new Vector2f(x / fw, y / fh);
points.Add(uv);
}
}
}
// distribute points on the boundary
for (int y = 0; y < img.height; y++)
{
for (int x = 0; x < img.width; x++)
{
if (EdgeDetect(img, x, y))
{
Vector2f uv = new Vector2f(x / fw, y / fh);
bpoints.Add(uv);
}
}
}
// triangulate
int iterations = 7;
List <Vector2f> verts;
List <int> tris;
Mesher.TriangulateVariational(points, bpoints, iterations, out verts, out tris);
//no longer used
points = null;
bpoints = null;
// discard triangles whose centroid is not inside the shape
for (int i = 0; i < tris.Count;)
{
Vector2f p = verts[tris[i + 0]];
Vector2f q = verts[tris[i + 1]];
Vector2f r = verts[tris[i + 2]];
Vector2f c = (p + q + r) / 3.0f;
//int x = Mathf.FloorToInt(c.x * fw);
//int y = Mathf.FloorToInt(c.y * fh);
//Color col = img.GetPixel(x, y);
Color col = img.GetPixelBilinear(c.x, c.y);
if (col.grayscale == 0.0f)
{
tris.RemoveRange(i, 3);
}
else
{
i += 3;
}
}
// generate particles
for (int i = 0; i < verts.Count; ++i)
{
Vector2f uv = verts[i];
Scene.Particles.Add(new FEMParticle(ToWorldPos(uv), uv, 0.0f));
}
// generate elements and assign mass based on connected area
for (int t = 0; t < tris.Count; t += 3)
{
int i = tris[t];
int j = tris[t + 1];
int k = tris[t + 2];
// calculate tri area
Vector2f a = Scene.Particles[i].p;
Vector2f b = Scene.Particles[j].p;
Vector2f c = Scene.Particles[k].p;
float area = 0.5f * Vector2f.Cross(b - a, c - a);
float mass = density * area / 3.0f;
Scene.Particles[i].invMass += mass;
Scene.Particles[j].invMass += mass;
Scene.Particles[k].invMass += mass;
Scene.Triangles.Add(new Triangle(i, j, k));
}
// convert mass to invmass
for (int i = 0; i < Scene.Particles.Count; ++i)
{
if (Scene.Particles[i].invMass > 0.0f)
{
Scene.Particles[i].invMass = 1.0f / Scene.Particles[i].invMass;
}
}
// assign index to particles
for (int i = 0; i < Scene.Particles.Count; ++i)
{
Scene.Particles[i].index = i;
}
Scene.CreateElements();
return(Scene);
}
private float CalcArea(Vector2f v1, Vector2f v2, Vector2f v3)
{
return((v1.Cross(v2) + v2.Cross(v3) + v3.Cross(v1)) * 0.5F);
}
