/**
* Clipping for contact manifolds. Sutherland-Hodgman clipping.
*
* @param vOut
* @param vIn
* @param normal
* @param offset
* @return
*/
public static final int clipSegmentToLine(
final ClipVertex[] vOut, final ClipVertex[] vIn, final Vec2 normal, float offset) {
// Start with no output points
int numOut = 0;
// Calculate the distance of end points to the line
float distance0 = Vec2.dot(normal, vIn[0].v) - offset;
float distance1 = Vec2.dot(normal, vIn[1].v) - offset;
// If the points are behind the plane
if (distance0 <= 0.0f) {
vOut[numOut++].set(vIn[0]);
}
if (distance1 <= 0.0f) {
vOut[numOut++].set(vIn[1]);
}
// If the points are on different sides of the plane
if (distance0 * distance1 < 0.0f) {
// Find intersection point of edge and plane
float interp = distance0 / (distance0 - distance1);
// vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v);
vOut[numOut].v.set(vIn[1].v).subLocal(vIn[0].v).mulLocal(interp).addLocal(vIn[0].v);
if (distance0 > 0.0f) {
vOut[numOut].id.set(vIn[0].id);
} else {
vOut[numOut].id.set(vIn[1].id);
}
++numOut;
}
return numOut;
}
/** @see SupportsGenericDistance#support(Vec2, XForm, Vec2) */
public void support(final Vec2 dest, final XForm xf, final Vec2 d) {
final Vec2 supportV1 = tlSupportV1.get();
final Vec2 supportV2 = tlSupportV2.get();
XForm.mulToOut(xf, m_coreV1, supportV1);
XForm.mulToOut(xf, m_coreV2, supportV2);
dest.set(Vec2.dot(supportV1, d) > Vec2.dot(supportV2, d) ? supportV1 : supportV2);
}
public float computeSubmergedArea(final Vec2 normal, float offset, XForm xf, Vec2 c) {
final Vec2 v0 = tlV0.get();
final Vec2 v1 = tlV1.get();
final Vec2 v2 = tlV2.get();
final Vec2 temp = tlTemp.get();
// Note that v0 is independent of any details of the specific edge
// We are relying on v0 being consistent between multiple edges of the same body
v0.set(normal).mul(offset);
// b2Vec2 v0 = xf.position + (offset - b2Dot(normal, xf.position)) * normal;
XForm.mulToOut(xf, m_v1, v1);
XForm.mulToOut(xf, m_v2, v2);
float d1 = Vec2.dot(normal, v1) - offset;
float d2 = Vec2.dot(normal, v2) - offset;
if (d1 > 0.0f) {
if (d2 > 0.0f) {
return 0.0f;
} else {
temp.set(v2).mulLocal(d1 / (d1 - d2));
v1.mulLocal(-d2 / (d1 - d2)).addLocal(temp);
}
} else {
if (d2 > 0.0f) {
temp.set(v1).mulLocal(-d2 / (d1 - d2));
v2.mulLocal(d1 / (d1 - d2)).addLocal(temp);
} else {
// Nothing
}
}
final Vec2 e1 = tlE1.get();
final Vec2 e2 = tlE2.get();
// v0,v1,v2 represents a fully submerged triangle
float k_inv3 = 1.0f / 3.0f;
// Area weighted centroid
c.x = k_inv3 * (v0.x + v1.x + v2.x);
c.y = k_inv3 * (v0.y + v1.y + v2.y);
e1.set(v1).subLocal(v0);
e2.set(v2).subLocal(v0);
return 0.5f * Vec2.cross(e1, e2);
}
/**
* Compute the collision manifold between two circles.
*
* @param manifold
* @param circle1
* @param xfA
* @param circle2
* @param xfB
*/
public final void collideCircles(
Manifold manifold,
final CircleShape circle1,
final Transform xfA,
final CircleShape circle2,
final Transform xfB) {
manifold.pointCount = 0;
Transform.mulToOut(xfA, circle1.m_p, pA);
Transform.mulToOut(xfB, circle2.m_p, pB);
d.set(pB).subLocal(pA);
float distSqr = Vec2.dot(d, d);
float radius = circle1.m_radius + circle2.m_radius;
if (distSqr > radius * radius) {
return;
}
manifold.type = ManifoldType.CIRCLES;
manifold.localPoint.set(circle1.m_p);
manifold.localNormal.setZero();
manifold.pointCount = 1;
manifold.points[0].localPoint.set(circle2.m_p);
manifold.points[0].id.zero();
}
@Override
public final boolean testPoint(final Transform xf, final Vec2 p) {
final Vec2 pLocal = pool1;
final Vec2 temp = pool2;
pLocal.set(p).subLocal(xf.p);
Rot.mulTransUnsafe(xf.q, pLocal, temp);
pLocal.set(temp);
if (m_debug) {
System.out.println("--testPoint debug--");
System.out.println("Vertices: ");
for (int i = 0; i < m_count; ++i) {
System.out.println(m_vertices[i]);
}
System.out.println("pLocal: " + pLocal);
}
for (int i = 0; i < m_count; ++i) {
temp.set(pLocal).subLocal(m_vertices[i]);
final float dot = Vec2.dot(m_normals[i], temp);
if (dot > 0.0f) {
return false;
}
}
return true;
}
/**
* Find the separation between poly1 and poly2 for a given edge normal on poly1.
*
* @param poly1
* @param xf1
* @param edge1
* @param poly2
* @param xf2
*/
public final float edgeSeparation(
final PolygonShape poly1,
final Transform xf1,
final int edge1,
final PolygonShape poly2,
final Transform xf2) {
int count1 = poly1.m_vertexCount;
final Vec2[] vertices1 = poly1.m_vertices;
final Vec2[] normals1 = poly1.m_normals;
int count2 = poly2.m_vertexCount;
final Vec2[] vertices2 = poly2.m_vertices;
assert (0 <= edge1 && edge1 < count1);
// Convert normal from poly1's frame into poly2's frame.
// Vec2 normal1World = Mul(xf1.R, normals1[edge1]);
Mat22.mulToOut(xf1.R, normals1[edge1], normal1World);
// Vec2 normal1 = MulT(xf2.R, normal1World);
Mat22.mulTransToOut(xf2.R, normal1World, normal1);
// Find support vertex on poly2 for -normal.
int index = 0;
float minDot = Float.MAX_VALUE;
for (int i = 0; i < count2; ++i) {
float dot = Vec2.dot(vertices2[i], normal1);
if (dot < minDot) {
minDot = dot;
index = i;
}
}
// Vec2 v1 = Mul(xf1, vertices1[edge1]);
// Vec2 v2 = Mul(xf2, vertices2[index]);
Transform.mulToOut(xf1, vertices1[edge1], v1);
Transform.mulToOut(xf2, vertices2[index], v2);
float separation = Vec2.dot(v2.subLocal(v1), normal1World);
return separation;
}
/**
* Set the linear velocity of the center of mass.
*
* @param v the new linear velocity of the center of mass.
*/
public final void setLinearVelocity(Vec2 v) {
if (m_type == BodyType.STATIC) {
return;
}
if (Vec2.dot(v, v) > 0.0f) {
setAwake(true);
}
m_linearVelocity.set(v);
}
/** @see Shape#testSegment(XForm, RaycastResult, Segment, float) */
@Override
public SegmentCollide testSegment(
final XForm xf, final RaycastResult out, final Segment segment, final float maxLambda) {
final Vec2 r = tlR.get();
final Vec2 v1 = tlV1.get();
final Vec2 d = tlD.get();
final Vec2 n = tlN.get();
final Vec2 b = tlB.get();
r.set(segment.p2).subLocal(segment.p1);
XForm.mulToOut(xf, m_v1, v1);
XForm.mulToOut(xf, m_v2, d);
d.subLocal(v1);
Vec2.crossToOut(d, 1.0f, n);
final float k_slop = 100.0f * Settings.EPSILON;
final float denom = -Vec2.dot(r, n);
// Cull back facing collision and ignore parallel segments.
if (denom > k_slop) {
// Does the segment intersect the infinite line associated with this segment?
b.set(segment.p1).subLocal(v1);
float a = Vec2.dot(b, n);
if (0.0f <= a && a <= maxLambda * denom) {
final float mu2 = -r.x * b.y + r.y * b.x;
// Does the segment intersect this segment?
if (-k_slop * denom <= mu2 && mu2 <= denom * (1.0f + k_slop)) {
a /= denom;
n.normalize();
out.lambda = a;
out.normal.set(n);
return SegmentCollide.HIT_COLLIDE;
}
}
}
return SegmentCollide.MISS_COLLIDE;
}
// djm pooling from above
public final void findIncidentEdge(
final ClipVertex[] c,
final PolygonShape poly1,
final Transform xf1,
int edge1,
final PolygonShape poly2,
final Transform xf2) {
int count1 = poly1.m_vertexCount;
final Vec2[] normals1 = poly1.m_normals;
int count2 = poly2.m_vertexCount;
final Vec2[] vertices2 = poly2.m_vertices;
final Vec2[] normals2 = poly2.m_normals;
assert (0 <= edge1 && edge1 < count1);
// Get the normal of the reference edge in poly2's frame.
Mat22.mulToOut(xf1.R, normals1[edge1], normal1); // temporary
// b2Vec2 normal1 = b2MulT(xf2.R, b2Mul(xf1.R, normals1[edge1]));
Mat22.mulTransToOut(xf2.R, normal1, normal1);
// Find the incident edge on poly2.
int index = 0;
float minDot = Float.MAX_VALUE;
for (int i = 0; i < count2; ++i) {
float dot = Vec2.dot(normal1, normals2[i]);
if (dot < minDot) {
minDot = dot;
index = i;
}
}
// Build the clip vertices for the incident edge.
int i1 = index;
int i2 = i1 + 1 < count2 ? i1 + 1 : 0;
Transform.mulToOut(xf2, vertices2[i1], c[0].v); // = Mul(xf2, vertices2[i1]);
c[0].id.features.referenceEdge = edge1;
c[0].id.features.incidentEdge = i1;
c[0].id.features.incidentVertex = 0;
Transform.mulToOut(xf2, vertices2[i2], c[1].v); // = Mul(xf2, vertices2[i2]);
c[1].id.features.referenceEdge = edge1;
c[1].id.features.incidentEdge = i2;
c[1].id.features.incidentVertex = 1;
}
/**
* Set the mass properties to override the mass properties of the fixtures. Note that this changes
* the center of mass position. Note that creating or destroying fixtures can also alter the mass.
* This function has no effect if the body isn't dynamic.
*
* @param massData the mass properties.
*/
public final void setMassData(MassData massData) {
// TODO_ERIN adjust linear velocity and torque to account for movement of center.
assert (m_world.isLocked() == false);
if (m_world.isLocked() == true) {
return;
}
if (m_type != BodyType.DYNAMIC) {
return;
}
m_invMass = 0.0f;
m_I = 0.0f;
m_invI = 0.0f;
m_mass = massData.mass;
if (m_mass <= 0.0f) {
m_mass = 1f;
}
m_invMass = 1.0f / m_mass;
if (massData.I > 0.0f && (m_flags & e_fixedRotationFlag) == 0) {
m_I = massData.I - m_mass * Vec2.dot(massData.center, massData.center);
assert (m_I > 0.0f);
m_invI = 1.0f / m_I;
}
final Vec2 oldCenter = m_world.getPool().popVec2();
// Move center of mass.
oldCenter.set(m_sweep.c);
m_sweep.localCenter.set(massData.center);
// m_sweep.c0 = m_sweep.c = Mul(m_xf, m_sweep.localCenter);
Transform.mulToOut(m_xf, m_sweep.localCenter, m_sweep.c0);
m_sweep.c.set(m_sweep.c0);
// Update center of mass velocity.
// m_linearVelocity += Cross(m_angularVelocity, m_sweep.c - oldCenter);
final Vec2 temp = m_world.getPool().popVec2();
temp.set(m_sweep.c).subLocal(oldCenter);
Vec2.crossToOut(m_angularVelocity, temp, temp);
m_linearVelocity.addLocal(temp);
m_world.getPool().pushVec2(2);
}
/**
* Compute the collision manifold between two polygons.
*
* @param manifold
* @param polygon1
* @param xf1
* @param polygon2
* @param xf2
*/
public final void collidePolygons(
Manifold manifold,
final PolygonShape polyA,
final Transform xfA,
final PolygonShape polyB,
final Transform xfB) {
// Find edge normal of max separation on A - return if separating axis is found
// Find edge normal of max separation on B - return if separation axis is found
// Choose reference edge as min(minA, minB)
// Find incident edge
// Clip
// The normal points from 1 to 2
manifold.pointCount = 0;
float totalRadius = polyA.m_radius + polyB.m_radius;
findMaxSeparation(results1, polyA, xfA, polyB, xfB);
if (results1.separation > totalRadius) {
return;
}
findMaxSeparation(results2, polyB, xfB, polyA, xfA);
if (results2.separation > totalRadius) {
return;
}
final PolygonShape poly1; // reference polygon
final PolygonShape poly2; // incident polygon
Transform xf1, xf2;
int edge1; // reference edge
int flip;
final float k_relativeTol = 0.98f;
final float k_absoluteTol = 0.001f;
if (results2.separation > k_relativeTol * results1.separation + k_absoluteTol) {
poly1 = polyB;
poly2 = polyA;
xf1 = xfB;
xf2 = xfA;
edge1 = results2.edgeIndex;
manifold.type = ManifoldType.FACE_B;
flip = 1;
} else {
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = results1.edgeIndex;
manifold.type = ManifoldType.FACE_A;
flip = 0;
}
findIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
int count1 = poly1.m_vertexCount;
final Vec2[] vertices1 = poly1.m_vertices;
v11.set(vertices1[edge1]);
v12.set(edge1 + 1 < count1 ? vertices1[edge1 + 1] : vertices1[0]);
localTangent.set(v12).subLocal(v11);
localTangent.normalize();
Vec2.crossToOut(localTangent, 1f, localNormal); // Vec2 localNormal = Cross(dv,
// 1.0f);
planePoint.set(v11).addLocal(v12).mulLocal(.5f); // Vec2 planePoint = 0.5f * (v11
// + v12);
Mat22.mulToOut(xf1.R, localTangent, tangent); // Vec2 sideNormal = Mul(xf1.R, v12
// - v11);
Vec2.crossToOut(tangent, 1f, normal); // Vec2 frontNormal = Cross(sideNormal,
// 1.0f);
Transform.mulToOut(xf1, v11, v11);
Transform.mulToOut(xf1, v12, v12);
// v11 = Mul(xf1, v11);
// v12 = Mul(xf1, v12);
// Face offset
float frontOffset = Vec2.dot(normal, v11);
// Side offsets, extended by polytope skin thickness.
float sideOffset1 = -Vec2.dot(tangent, v11) + totalRadius;
float sideOffset2 = Vec2.dot(tangent, v12) + totalRadius;
// Clip incident edge against extruded edge1 side edges.
// ClipVertex clipPoints1[2];
// ClipVertex clipPoints2[2];
int np;
// Clip to box side 1
// np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, sideOffset1);
tangent.negateLocal();
np = clipSegmentToLine(clipPoints1, incidentEdge, tangent, sideOffset1);
tangent.negateLocal();
if (np < 2) {
return;
}
// Clip to negative box side 1
np = clipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2);
if (np < 2) {
return;
}
// Now clipPoints2 contains the clipped points.
manifold.localNormal.set(localNormal);
manifold.localPoint.set(planePoint);
int pointCount = 0;
for (int i = 0; i < Settings.maxManifoldPoints; ++i) {
float separation = Vec2.dot(normal, clipPoints2[i].v) - frontOffset;
if (separation <= totalRadius) {
ManifoldPoint cp = manifold.points[pointCount];
Transform.mulTransToOut(xf2, clipPoints2[i].v, cp.localPoint);
// cp.m_localPoint = MulT(xf2, clipPoints2[i].v);
cp.id.set(clipPoints2[i].id);
cp.id.features.flip = flip;
++pointCount;
}
}
manifold.pointCount = pointCount;
}
/**
* Find the max separation between poly1 and poly2 using edge normals from poly1.
*
* @param edgeIndex
* @param poly1
* @param xf1
* @param poly2
* @param xf2
* @return
*/
public final void findMaxSeparation(
EdgeResults results,
final PolygonShape poly1,
final Transform xf1,
final PolygonShape poly2,
final Transform xf2) {
int count1 = poly1.m_vertexCount;
final Vec2[] normals1 = poly1.m_normals;
// Vector pointing from the centroid of poly1 to the centroid of poly2.
Transform.mulToOut(xf2, poly2.m_centroid, d);
Transform.mulToOut(xf1, poly1.m_centroid, temp);
d.subLocal(temp);
Mat22.mulTransToOut(xf1.R, d, dLocal1);
// Find edge normal on poly1 that has the largest projection onto d.
int edge = 0;
float dot;
float maxDot = Float.MIN_VALUE;
for (int i = 0; i < count1; i++) {
dot = Vec2.dot(normals1[i], dLocal1);
if (dot > maxDot) {
maxDot = dot;
edge = i;
}
}
// Get the separation for the edge normal.
float s = edgeSeparation(poly1, xf1, edge, poly2, xf2);
// Check the separation for the previous edge normal.
int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
float sPrev = edgeSeparation(poly1, xf1, prevEdge, poly2, xf2);
// Check the separation for the next edge normal.
int nextEdge = edge + 1 < count1 ? edge + 1 : 0;
float sNext = edgeSeparation(poly1, xf1, nextEdge, poly2, xf2);
// Find the best edge and the search direction.
int bestEdge;
float bestSeparation;
int increment;
if (sPrev > s && sPrev > sNext) {
increment = -1;
bestEdge = prevEdge;
bestSeparation = sPrev;
} else if (sNext > s) {
increment = 1;
bestEdge = nextEdge;
bestSeparation = sNext;
} else {
results.edgeIndex = edge;
results.separation = s;
return;
}
// Perform a local search for the best edge normal.
for (; ; ) {
if (increment == -1) {
edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
} else {
edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;
}
s = edgeSeparation(poly1, xf1, edge, poly2, xf2);
if (s > bestSeparation) {
bestEdge = edge;
bestSeparation = s;
} else {
break;
}
}
results.edgeIndex = bestEdge;
results.separation = bestSeparation;
}
/**
* Compute the collision manifold between a polygon and a circle.
*
* @param manifold
* @param polygon
* @param xfA
* @param circle
* @param xfB
*/
public final void collidePolygonAndCircle(
Manifold manifold,
final PolygonShape polygon,
final Transform xfA,
final CircleShape circle,
final Transform xfB) {
manifold.pointCount = 0;
// Compute circle position in the frame of the polygon.
Transform.mulToOut(xfB, circle.m_p, c);
Transform.mulTransToOut(xfA, c, cLocal);
// Find the min separating edge.
int normalIndex = 0;
float separation = Float.MIN_VALUE;
float radius = polygon.m_radius + circle.m_radius;
int vertexCount = polygon.m_vertexCount;
Vec2[] vertices = polygon.m_vertices;
Vec2[] normals = polygon.m_normals;
for (int i = 0; i < vertexCount; i++) {
temp.set(cLocal).subLocal(vertices[i]);
float s = Vec2.dot(normals[i], temp);
if (s > radius) {
// early out
return;
}
if (s > separation) {
separation = s;
normalIndex = i;
}
}
// Vertices that subtend the incident face.
int vertIndex1 = normalIndex;
int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
Vec2 v1 = vertices[vertIndex1];
Vec2 v2 = vertices[vertIndex2];
// If the center is inside the polygon ...
if (separation < Settings.EPSILON) {
manifold.pointCount = 1;
manifold.type = ManifoldType.FACE_A;
manifold.localNormal.set(normals[normalIndex]);
manifold.localPoint.set(v1).addLocal(v2).mulLocal(.5f);
manifold.points[0].localPoint.set(circle.m_p);
manifold.points[0].id.zero();
return;
}
// Compute barycentric coordinates
temp.set(cLocal).subLocal(v1);
temp2.set(v2).subLocal(v1);
float u1 = Vec2.dot(temp, temp2);
temp.set(cLocal).subLocal(v2);
temp2.set(v1).subLocal(v2);
float u2 = Vec2.dot(temp, temp2);
if (u1 <= 0f) {
if (MathUtils.distanceSquared(cLocal, v1) > radius * radius) {
return;
}
manifold.pointCount = 1;
manifold.type = ManifoldType.FACE_A;
manifold.localNormal.set(cLocal).subLocal(v1);
manifold.localNormal.normalize();
manifold.localPoint.set(v1);
manifold.points[0].localPoint.set(circle.m_p);
manifold.points[0].id.zero();
} else if (u2 <= 0.0f) {
if (MathUtils.distanceSquared(cLocal, v2) > radius * radius) {
return;
}
manifold.pointCount = 1;
manifold.type = ManifoldType.FACE_A;
manifold.localNormal.set(cLocal).subLocal(v2);
manifold.localNormal.normalize();
manifold.localPoint.set(v2);
manifold.points[0].localPoint.set(circle.m_p);
manifold.points[0].id.zero();
} else {
// Vec2 faceCenter = 0.5f * (v1 + v2);
// (temp is faceCenter)
temp.set(v1).addLocal(v2).mulLocal(.5f);
temp2.set(cLocal).subLocal(temp);
separation = Vec2.dot(temp2, normals[vertIndex1]);
if (separation > radius) {
return;
}
manifold.pointCount = 1;
manifold.type = ManifoldType.FACE_A;
manifold.localNormal.set(normals[vertIndex1]);
manifold.localPoint.set(temp); // (faceCenter)
manifold.points[0].localPoint.set(circle.m_p);
manifold.points[0].id.zero();
}
}
public void computeMass(final MassData massData, float density) {
// Polygon mass, centroid, and inertia.
// Let rho be the polygon density in mass per unit area.
// Then:
// mass = rho * int(dA)
// centroid.x = (1/mass) * rho * int(x * dA)
// centroid.y = (1/mass) * rho * int(y * dA)
// I = rho * int((x*x + y*y) * dA)
//
// We can compute these integrals by summing all the integrals
// for each triangle of the polygon. To evaluate the integral
// for a single triangle, we make a change of variables to
// the (u,v) coordinates of the triangle:
// x = x0 + e1x * u + e2x * v
// y = y0 + e1y * u + e2y * v
// where 0 <= u && 0 <= v && u + v <= 1.
//
// We integrate u from [0,1-v] and then v from [0,1].
// We also need to use the Jacobian of the transformation:
// D = cross(e1, e2)
//
// Simplification: triangle centroid = (1/3) * (p1 + p2 + p3)
//
// The rest of the derivation is handled by computer algebra.
assert (m_count >= 3);
final Vec2 center = pool1;
center.setZero();
float area = 0.0f;
float I = 0.0f;
// pRef is the reference point for forming triangles.
// It's location doesn't change the result (except for rounding error).
final Vec2 s = pool2;
s.setZero();
// This code would put the reference point inside the polygon.
for (int i = 0; i < m_count; ++i) {
s.addLocal(m_vertices[i]);
}
s.mulLocal(1.0f / m_count);
final float k_inv3 = 1.0f / 3.0f;
final Vec2 e1 = pool3;
final Vec2 e2 = pool4;
for (int i = 0; i < m_count; ++i) {
// Triangle vertices.
e1.set(m_vertices[i]).subLocal(s);
e2.set(s).negateLocal().addLocal(i + 1 < m_count ? m_vertices[i + 1] : m_vertices[0]);
final float D = Vec2.cross(e1, e2);
final float triangleArea = 0.5f * D;
area += triangleArea;
// Area weighted centroid
center.x += triangleArea * k_inv3 * (e1.x + e2.x);
center.y += triangleArea * k_inv3 * (e1.y + e2.y);
final float ex1 = e1.x, ey1 = e1.y;
final float ex2 = e2.x, ey2 = e2.y;
float intx2 = ex1 * ex1 + ex2 * ex1 + ex2 * ex2;
float inty2 = ey1 * ey1 + ey2 * ey1 + ey2 * ey2;
I += (0.25f * k_inv3 * D) * (intx2 + inty2);
}
// Total mass
massData.mass = density * area;
// Center of mass
assert (area > Settings.EPSILON);
center.mulLocal(1.0f / area);
massData.center.set(center).addLocal(s);
// Inertia tensor relative to the local origin (point s)
massData.I = I * density;
// Shift to center of mass then to original body origin.
massData.I += massData.mass * (Vec2.dot(massData.center, massData.center));
}
@Override
public final boolean raycast(
RayCastOutput output, RayCastInput input, Transform xf, int childIndex) {
final Vec2 p1 = pool1;
final Vec2 p2 = pool2;
final Vec2 d = pool3;
final Vec2 temp = pool4;
p1.set(input.p1).subLocal(xf.p);
Rot.mulTrans(xf.q, p1, p1);
p2.set(input.p2).subLocal(xf.p);
Rot.mulTrans(xf.q, p2, p2);
d.set(p2).subLocal(p1);
// if (count == 2) {
// } else {
float lower = 0, upper = input.maxFraction;
int index = -1;
for (int i = 0; i < m_count; ++i) {
// p = p1 + a * d
// dot(normal, p - v) = 0
// dot(normal, p1 - v) + a * dot(normal, d) = 0
temp.set(m_vertices[i]).subLocal(p1);
final float numerator = Vec2.dot(m_normals[i], temp);
final float denominator = Vec2.dot(m_normals[i], d);
if (denominator == 0.0f) {
if (numerator < 0.0f) {
return false;
}
} else {
// Note: we want this predicate without division:
// lower < numerator / denominator, where denominator < 0
// Since denominator < 0, we have to flip the inequality:
// lower < numerator / denominator <==> denominator * lower >
// numerator.
if (denominator < 0.0f && numerator < lower * denominator) {
// Increase lower.
// The segment enters this half-space.
lower = numerator / denominator;
index = i;
} else if (denominator > 0.0f && numerator < upper * denominator) {
// Decrease upper.
// The segment exits this half-space.
upper = numerator / denominator;
}
}
if (upper < lower) {
return false;
}
}
assert (0.0f <= lower && lower <= input.maxFraction);
if (index >= 0) {
output.fraction = lower;
Rot.mulToOutUnsafe(xf.q, m_normals[index], output.normal);
// normal = Mul(xf.R, m_normals[index]);
return true;
}
return false;
}
/**
* This resets the mass properties to the sum of the mass properties of the fixtures. This
* normally does not need to be called unless you called setMassData to override the mass and you
* later want to reset the mass.
*/
public final void resetMassData() {
// Compute mass data from shapes. Each shape has its own density.
m_mass = 0.0f;
m_invMass = 0.0f;
m_I = 0.0f;
m_invI = 0.0f;
m_sweep.localCenter.setZero();
// Static and kinematic bodies have zero mass.
if (m_type == BodyType.STATIC || m_type == BodyType.KINEMATIC) {
// m_sweep.c0 = m_sweep.c = m_xf.position;
m_sweep.c.set(m_xf.position);
m_sweep.c0.set(m_xf.position);
return;
}
assert (m_type == BodyType.DYNAMIC);
// Accumulate mass over all fixtures.
final Vec2 center = m_world.getPool().popVec2();
center.setZero();
final Vec2 temp = m_world.getPool().popVec2();
final MassData massData = pmd;
for (Fixture f = m_fixtureList; f != null; f = f.m_next) {
if (f.m_density == 0.0f) {
continue;
}
f.getMassData(massData);
m_mass += massData.mass;
// center += massData.mass * massData.center;
temp.set(massData.center).mulLocal(massData.mass);
center.addLocal(temp);
m_I += massData.I;
}
// Compute center of mass.
if (m_mass > 0.0f) {
m_invMass = 1.0f / m_mass;
center.mulLocal(m_invMass);
} else {
// Force all dynamic bodies to have a positive mass.
m_mass = 1.0f;
m_invMass = 1.0f;
}
if (m_I > 0.0f && (m_flags & e_fixedRotationFlag) == 0) {
// Center the inertia about the center of mass.
m_I -= m_mass * Vec2.dot(center, center);
assert (m_I > 0.0f);
m_invI = 1.0f / m_I;
} else {
m_I = 0.0f;
m_invI = 0.0f;
}
Vec2 oldCenter = m_world.getPool().popVec2();
// Move center of mass.
oldCenter.set(m_sweep.c);
m_sweep.localCenter.set(center);
// m_sweep.c0 = m_sweep.c = Mul(m_xf, m_sweep.localCenter);
Transform.mulToOut(m_xf, m_sweep.localCenter, m_sweep.c0);
m_sweep.c.set(m_sweep.c0);
// Update center of mass velocity.
// m_linearVelocity += Cross(m_angularVelocity, m_sweep.c - oldCenter);
temp.set(m_sweep.c).subLocal(oldCenter);
Vec2.crossToOut(m_angularVelocity, temp, temp);
m_linearVelocity.addLocal(temp);
m_world.getPool().pushVec2(3);
}
