|
I ported over B2PolyAndEdgeContact.hx and made a tweak to B2EdgeShape.hx, based off the current C++ code athttp://code.google.com/p/box2d/source/browse/trunk/Box2D/Box2D/Coll...
The good news is that the porting wasn't terribly difficult. The bad news is that it still doesn't register collisions for the Polygon-Edge case. I've tried to debug this for several days now and am close to giving up, despite being so close.
It always fails at the conditional right before "Never gets past this point" - if that's worth anything.
Any help would be much, much appreciated. You have to apply all the changes in the prior post, and then replace the 2 following files.
1) Replace B2EdgeShape.hx with this. These are additional fields required to store adjacency info.
package box2D.collision.shapes;
import box2D.collision.B2AABB;
import box2D.collision.B2RayCastInput;
import box2D.collision.B2RayCastOutput;
import box2D.common.B2Settings;
import box2D.common.math.B2Mat22;
import box2D.common.math.B2Math;
import box2D.common.math.B2Transform;
import box2D.common.math.B2Vec2;
/**
* An edge shape.
* @private
* @see b2EdgeChainDef
*/
class B2EdgeShape extends B2Shape
{
/**
* Returns false. Edges cannot contain points.
*/
public override function testPoint(transform:B2Transform, p:B2Vec2) : Bool{
return false;
}
/**
* @inheritDoc
*/
public override function rayCast(output:B2RayCastOutput, input:B2RayCastInput, transform:B2Transform):Bool
{
var tMat:B2Mat22;
var rX: Float = input.p2.x - input.p1.x;
var rY: Float = input.p2.y - input.p1.y;
//b2Vec2 v1 = b2Mul(transform, m_v1);
tMat = transform.R;
var v1X: Float = transform.position.x + (tMat.col1.x * m_v1.x + tMat.col2.x * m_v1.y);
var v1Y: Float = transform.position.y + (tMat.col1.y * m_v1.x + tMat.col2.y * m_v1.y);
//b2Vec2 n = b2Cross(d, 1.0);
var nX: Float = transform.position.y + (tMat.col1.y * m_v2.x + tMat.col2.y * m_v2.y) - v1Y;
var nY: Float = -(transform.position.x + (tMat.col1.x * m_v2.x + tMat.col2.x * m_v2.y) - v1X);
var k_slop: Float = 100.0 * B2Math.MIN_VALUE;
var denom: Float = -(rX * nX + rY * nY);
// Cull back facing collision and ignore parallel segments.
if (denom > k_slop)
{
// Does the segment intersect the infinite line associated with this segment?
var bX: Float = input.p1.x - v1X;
var bY: Float = input.p1.y - v1Y;
var a: Float = (bX * nX + bY * nY);
if (0.0 <= a && a <= input.maxFraction * denom)
{
var mu2: Float = -rX * bY + rY * bX;
// Does the segment intersect this segment?
if (-k_slop * denom <= mu2 && mu2 <= denom * (1.0 + k_slop))
{
a /= denom;
output.fraction = a;
var nLen: Float = Math.sqrt(nX * nX + nY * nY);
output.normal.x = nX / nLen;
output.normal.y = nY / nLen;
return true;
}
}
}
return false;
}
/**
* @inheritDoc
*/
public override function computeAABB(aabb:B2AABB, transform:B2Transform) : Void{
var tMat:B2Mat22 = transform.R;
//b2Vec2 v1 = b2Mul(transform, m_v1);
var v1X:Float = transform.position.x + (tMat.col1.x * m_v1.x + tMat.col2.x * m_v1.y);
var v1Y:Float = transform.position.y + (tMat.col1.y * m_v1.x + tMat.col2.y * m_v1.y);
//b2Vec2 v2 = b2Mul(transform, m_v2);
var v2X:Float = transform.position.x + (tMat.col1.x * m_v2.x + tMat.col2.x * m_v2.y);
var v2Y:Float = transform.position.y + (tMat.col1.y * m_v2.x + tMat.col2.y * m_v2.y);
if (v1X < v2X) {
aabb.lowerBound.x = v1X;
aabb.upperBound.x = v2X;
} else {
aabb.lowerBound.x = v2X;
aabb.upperBound.x = v1X;
}
if (v1Y < v2Y) {
aabb.lowerBound.y = v1Y;
aabb.upperBound.y = v2Y;
} else {
aabb.lowerBound.y = v2Y;
aabb.upperBound.y = v1Y;
}
}
/**
* @inheritDoc
*/
public override function computeMass(massData:B2MassData, density:Float) : Void{
massData.mass = 0;
massData.center.setV(m_v1);
massData.I = 0;
}
/**
* @inheritDoc
*/
public override function computeSubmergedArea(
normal:B2Vec2,
offset:Float,
xf:B2Transform,
c:B2Vec2):Float
{
// Note that v0 is independant of any details of the specific edge
// We are relying on v0 being consistent between multiple edges of the same body
//b2Vec2 v0 = offset * normal;
var v0:B2Vec2 = new B2Vec2(normal.x * offset, normal.y * offset);
var v1:B2Vec2 = B2Math.mulX(xf, m_v1, true);
var v2:B2Vec2 = B2Math.mulX(xf, m_v2, true);
var d1:Float = B2Math.dot(normal, v1) - offset;
var d2:Float = B2Math.dot(normal, v2) - offset;
if (d1 > 0)
{
if (d2 > 0)
{
return 0;
}
else
{
//v1 = -d2 / (d1 - d2) * v1 + d1 / (d1 - d2) * v2;
v1.x = -d2 / (d1 - d2) * v1.x + d1 / (d1 - d2) * v2.x;
v1.y = -d2 / (d1 - d2) * v1.y + d1 / (d1 - d2) * v2.y;
}
}
else
{
if (d2 > 0)
{
//v2 = -d2 / (d1 - d2) * v1 + d1 / (d1 - d2) * v2;
v2.x = -d2 / (d1 - d2) * v1.x + d1 / (d1 - d2) * v2.x;
v2.y = -d2 / (d1 - d2) * v1.y + d1 / (d1 - d2) * v2.y;
}
else
{
// Nothing
}
}
// v0,v1,v2 represents a fully submerged triangle
// Area weighted centroid
c.x = (v0.x + v1.x + v2.x) / 3;
c.y = (v0.y + v1.y + v2.y) / 3;
//b2Vec2 e1 = v1 - v0;
//b2Vec2 e2 = v2 - v0;
//return 0.5f * b2Cross(e1, e2);
return 0.5 * ( (v1.x - v0.x) * (v2.y - v0.y) - (v1.y - v0.y) * (v2.x - v0.x) );
}
/**
* Get the distance from vertex1 to vertex2.
*/
public function getLength(): Float
{
return m_length;
}
/**
* Get the local position of vertex1 in parent body.
*/
public function getVertex1(): B2Vec2
{
return m_v1;
}
/**
* Get the local position of vertex2 in parent body.
*/
public function getVertex2(): B2Vec2
{
return m_v2;
}
/**
* Get a core vertex in local coordinates. These vertices
* represent a smaller edge that is used for time of impact
* computations.
*/
public function getCoreVertex1(): B2Vec2
{
return m_coreV1;
}
/**
* Get a core vertex in local coordinates. These vertices
* represent a smaller edge that is used for time of impact
* computations.
*/
public function getCoreVertex2(): B2Vec2
{
return m_coreV2;
}
/**
* Get a perpendicular unit vector, pointing
* from the solid side to the empty side.
*/
public function getNormalVector(): B2Vec2
{
return m_normal;
}
/**
* Get a parallel unit vector, pointing
* from vertex1 to vertex2.
*/
public function getDirectionVector(): B2Vec2
{
return m_direction;
}
/**
* Returns a unit vector halfway between
* m_direction and m_prevEdge.m_direction.
*/
public function getCorner1Vector(): B2Vec2
{
return m_cornerDir1;
}
/**
* Returns a unit vector halfway between
* m_direction and m_nextEdge.m_direction.
*/
public function getCorner2Vector(): B2Vec2
{
return m_cornerDir2;
}
/**
* Returns true if the first corner of this edge
* bends towards the solid side.
*/
public function corner1IsConvex(): Bool
{
return m_cornerConvex1;
}
/**
* Returns true if the second corner of this edge
* bends towards the solid side.
*/
public function corner2IsConvex(): Bool
{
return m_cornerConvex2;
}
/**
* Get the first vertex and apply the supplied transform.
*/
public function getFirstVertex(xf: B2Transform): B2Vec2
{
//return b2Mul(xf, m_coreV1);
var tMat:B2Mat22 = xf.R;
return new B2Vec2(xf.position.x + (tMat.col1.x * m_coreV1.x + tMat.col2.x * m_coreV1.y),
xf.position.y + (tMat.col1.y * m_coreV1.x + tMat.col2.y * m_coreV1.y));
}
/**
* Get the next edge in the chain.
*/
public function getNextEdge(): B2EdgeShape
{
return m_nextEdge;
}
/**
* Get the previous edge in the chain.
*/
public function getPrevEdge(): B2EdgeShape
{
return m_prevEdge;
}
private var s_supportVec:B2Vec2;
/**
* Get the support point in the given world direction.
* Use the supplied transform.
*/
public function support(xf:B2Transform, dX:Float, dY:Float) : B2Vec2{
var tMat:B2Mat22 = xf.R;
//b2Vec2 v1 = b2Mul(xf, m_coreV1);
var v1X:Float = xf.position.x + (tMat.col1.x * m_coreV1.x + tMat.col2.x * m_coreV1.y);
var v1Y:Float = xf.position.y + (tMat.col1.y * m_coreV1.x + tMat.col2.y * m_coreV1.y);
//b2Vec2 v2 = b2Mul(xf, m_coreV2);
var v2X:Float = xf.position.x + (tMat.col1.x * m_coreV2.x + tMat.col2.x * m_coreV2.y);
var v2Y:Float = xf.position.y + (tMat.col1.y * m_coreV2.x + tMat.col2.y * m_coreV2.y);
if ((v1X * dX + v1Y * dY) > (v2X * dX + v2Y * dY)) {
s_supportVec.x = v1X;
s_supportVec.y = v1Y;
} else {
s_supportVec.x = v2X;
s_supportVec.y = v2Y;
}
return s_supportVec;
}
//--------------- Internals Below -------------------
override public function copy():B2Shape
{
var s:B2Shape = new B2EdgeShape(m_v1, m_v2);
s.set(this);
var edge = cast(s, B2EdgeShape);
edge.m_v0.setV(m_v0);
edge.m_v3.setV(m_v3);
edge.m_hasVertex0 = m_hasVertex0;
edge.m_hasVertex3 = m_hasVertex3;
return s;
}
/**
* @private
*/
public function new (v1: B2Vec2, v2: B2Vec2){
super();
s_supportVec = new B2Vec2();
m_v1 = new B2Vec2();
m_v2 = new B2Vec2();
m_v0 = new B2Vec2();
m_v3 = new B2Vec2();
m_hasVertex0 = false;
m_hasVertex3 = false;
m_coreV1 = new B2Vec2();
m_coreV2 = new B2Vec2();
m_normal = new B2Vec2();
m_direction = new B2Vec2();
m_cornerDir1 = new B2Vec2();
m_cornerDir2 = new B2Vec2();
m_type = B2Shape.e_edgeShape;
m_prevEdge = null;
m_nextEdge = null;
m_v1 = v1;
m_v2 = v2;
m_direction.set(m_v2.x - m_v1.x, m_v2.y - m_v1.y);
m_length = m_direction.normalize();
m_normal.set(m_direction.y, -m_direction.x);
m_coreV1.set(-B2Settings.b2_toiSlop * (m_normal.x - m_direction.x) + m_v1.x,
-B2Settings.b2_toiSlop * (m_normal.y - m_direction.y) + m_v1.y);
m_coreV2.set(-B2Settings.b2_toiSlop * (m_normal.x + m_direction.x) + m_v2.x,
-B2Settings.b2_toiSlop * (m_normal.y + m_direction.y) + m_v2.y);
m_cornerDir1 = m_normal;
m_cornerDir2.set(-m_normal.x, -m_normal.y);
}
/**
* @private
*/
public function setPrevEdge(edge: B2EdgeShape, core: B2Vec2, cornerDir: B2Vec2, convex: Bool): Void
{
m_prevEdge = edge;
m_coreV1 = core;
m_cornerDir1 = cornerDir;
m_cornerConvex1 = convex;
}
/**
* @private
*/
public function setNextEdge(edge: B2EdgeShape, core: B2Vec2, cornerDir: B2Vec2, convex: Bool): Void
{
m_nextEdge = edge;
m_coreV2 = core;
m_cornerDir2 = cornerDir;
m_cornerConvex2 = convex;
}
public var m_v1:B2Vec2;
public var m_v2:B2Vec2;
//SMOOTH COLLISION
public var m_v0:B2Vec2;
public var m_v3:B2Vec2;
public var m_hasVertex0:Bool;
public var m_hasVertex3:Bool;
//END SMOOTH COLLISION
public var m_coreV1:B2Vec2;
public var m_coreV2:B2Vec2;
public var m_length:Float;
public var m_normal:B2Vec2;
public var m_direction:B2Vec2;
public var m_cornerDir1:B2Vec2;
public var m_cornerDir2:B2Vec2;
public var m_cornerConvex1:Bool;
public var m_cornerConvex2:Bool;
public var m_nextEdge:B2EdgeShape;
public var m_prevEdge:B2EdgeShape;
}
2) Replace B2PolyAndEdgeContact.hx with this. This is the meat of the implementation.
package box2D.dynamics.contacts;
import box2D.collision.B2ContactID;
import box2D.collision.B2Manifold;
import box2D.collision.B2ManifoldPoint;
import box2D.collision.shapes.B2EdgeShape;
import box2D.collision.shapes.B2PolygonShape;
import box2D.collision.shapes.B2Shape;
import box2D.common.math.B2Math;
import box2D.common.math.B2Mat22;
import box2D.common.B2Settings;
import box2D.common.math.B2Transform;
import box2D.common.math.B2Vec2;
import box2D.dynamics.B2Body;
import box2D.dynamics.B2Fixture;
class B2PolyAndEdgeContact extends B2Contact
{
static var m_xf:B2Transform = new B2Transform();
static var temp:B2Vec2 = new B2Vec2();
static var m_centroidB:B2Vec2 = new B2Vec2();
static var m_lowerLimit:B2Vec2 = new B2Vec2();
static var m_upperLimit:B2Vec2 = new B2Vec2();
static var m_polygonB:TempPolygon = new TempPolygon();
static var edgeAxis:EPAxis = new EPAxis();
static var polygonAxis:EPAxis = new EPAxis();
static var perp:B2Vec2 = new B2Vec2();
static var n:B2Vec2 = new B2Vec2();
static var rf:ReferenceFace = new ReferenceFace();
static var ie:Array<ClipVertex> = [new ClipVertex(), new ClipVertex()];
static var clipPoints1:Array<ClipVertex> = [new ClipVertex(), new ClipVertex()];
static var clipPoints2:Array<ClipVertex> = [new ClipVertex(), new ClipVertex()];
var m_v0:B2Vec2;
var m_v1:B2Vec2;
var m_v2:B2Vec2;
var m_v3:B2Vec2;
static var edge0:B2Vec2 = new B2Vec2();
static var edge1:B2Vec2 = new B2Vec2();
static var edge2:B2Vec2 = new B2Vec2();
static var m_normal:B2Vec2 = new B2Vec2();
static var m_normal0:B2Vec2 = new B2Vec2();
static var m_normal1:B2Vec2 = new B2Vec2();
static var m_normal2:B2Vec2 = new B2Vec2();
var m_front:Bool;
var m_radius:Float;
static public function create(allocator ynamic):B2Contact
{
return new B2PolyAndEdgeContact();
}
static public function destroy(contact:B2Contact, allocatorynamic):Void
{
}
public override function reset(fixtureA:B2Fixture = null, fixtureB:B2Fixture = null):Void
{
super.reset(fixtureA, fixtureB);
B2Settings.b2Assert(fixtureA.getType() == B2Shape.e_polygonShape);
B2Settings.b2Assert(fixtureB.getType() == B2Shape.e_edgeShape);
}
public override function evaluate():Void
{
var bA:B2Body = m_fixtureA.getBody();
var bB:B2Body = m_fixtureB.getBody();
b2CollidePolyAndEdge(m_manifold,
cast(m_fixtureA.getShape(), B2PolygonShape), bA.m_xf,
cast(m_fixtureB.getShape(), B2EdgeShape), bB.m_xf);
}
private function b2CollidePolyAndEdge(manifold:B2Manifold,
polygonB:B2PolygonShape,
xfA:B2Transform,
edgeA:B2EdgeShape,
xfB:B2Transform):Void
{
//m_xf = b2MulT(xfA, xfB);
//m_centroidB = b2Mul(m_xf, polygonB->m_centroid);
//TODO: Suspicious?
multiplyTransformsInverse(xfA, xfB, m_xf);
multiplyTransformVector(m_xf, polygonB.m_centroid, temp);
m_centroidB.setV(temp);
m_v0 = edgeA.m_v0;
m_v1 = edgeA.m_v1;
m_v2 = edgeA.m_v2;
m_v3 = edgeA.m_v3;
//boolean hasVertex0 = edgeA.m_hasVertex0;
//boolean hasVertex3 = edgeA.m_hasVertex3;
var hasVertex0 = edgeA.m_hasVertex0;
var hasVertex3 = edgeA.m_hasVertex3;
//edge1.set(m_v2).subLocal(m_v1);
//edge1.normalize();
//m_normal1.set(edge1.y, -edge1.x);
edge1.setV(m_v2);
edge1.subtract(m_v1);
edge1.normalize();
m_normal1.set(edge1.y, -edge1.x);
//float offset1 = Vec2.dot(m_normal1, temp.set(m_centroidB).subLocal(m_v1));
//float offset0 = 0.0f, offset2 = 0.0f;
//boolean convex1 = false, convex2 = false;
temp.setV(m_centroidB);
temp.subtract(m_v1);
var offset1 = B2Math.dot(m_normal1, temp);
var offset0:Float = 0.0;
var offset2:Float = 0.0;
var convex1 = false;
var convex2 = false;
//-----------------------------
//Is there a preceding edge?
if(hasVertex0)
{
edge0.setV(m_v1);
edge0.subtract(m_v0);
edge0.normalize();
m_normal0.set(edge0.y, -edge0.x);
convex1 = B2Math.crossVV(edge0, edge1) >= 0.0;
temp.setV(m_centroidB);
temp.subtract(m_v0);
offset0 = B2Math.dot(m_normal0, temp);
}
//Is there a following edge?
if(hasVertex3)
{
edge2.setV(m_v3);
edge2.subtract(m_v2);
edge2.normalize();
m_normal2.set(edge2.y, -edge2.x);
convex2 = B2Math.crossVV(edge1, edge2) > 0.0;
temp.setV(m_centroidB);
temp.subtract(m_v2);
offset2 = B2Math.dot(m_normal2, temp);
}
//-----------------------------
//Determine front or back collision. Determine collision normal limits.
if(hasVertex0 && hasVertex3)
{
if(convex1 && convex2)
{
m_front = offset0 >= 0.0 || offset1 >= 0.0 || offset2 >= 0.0;
if(m_front)
{
m_normal.setV(m_normal1);
m_lowerLimit.setV(m_normal0);
m_upperLimit.setV(m_normal2);
}
else
{
m_normal.setV(m_normal1);
m_normal.negativeSelf();
m_lowerLimit.setV(m_normal1);
m_lowerLimit.negativeSelf();
m_upperLimit.setV(m_normal1);
m_upperLimit.negativeSelf();
}
}
else if(convex1)
{
m_front = offset0 >= 0.0 || (offset1 >= 0.0 && offset2 >= 0.0);
if(m_front)
{
m_normal.setV(m_normal1);
m_lowerLimit.setV(m_normal0);
m_upperLimit.setV(m_normal1);
}
else
{
m_normal.setV(m_normal1);
m_normal.negativeSelf();
m_lowerLimit.setV(m_normal2);
m_lowerLimit.negativeSelf();
m_upperLimit.setV(m_normal1);
m_upperLimit.negativeSelf();
}
}
else if(convex2)
{
m_front = offset2 >= 0.0 || (offset0 >= 0.0 && offset1 >= 0.0);
if(m_front)
{
m_normal.setV(m_normal1);
m_lowerLimit.setV(m_normal1);
m_upperLimit.setV(m_normal2);
}
else
{
m_normal.setV(m_normal1);
m_normal.negativeSelf();
m_lowerLimit.setV(m_normal1);
m_lowerLimit.negativeSelf();
m_upperLimit.setV(m_normal0);
m_upperLimit.negativeSelf();
}
}
else
{
m_front = offset0 >= 0.0 && offset1 >= 0.0 && offset2 >= 0.0;
if(m_front)
{
m_normal.setV(m_normal1);
m_lowerLimit.setV(m_normal1);
m_upperLimit.setV(m_normal1);
}
else
{
m_normal.setV(m_normal1);
m_normal.negativeSelf();
m_lowerLimit.setV(m_normal2);
m_lowerLimit.negativeSelf();
m_upperLimit.setV(m_normal0);
m_upperLimit.negativeSelf();
}
}
}
else if(hasVertex0)
{
if(convex1)
{
m_front = offset0 >= 0.0 || offset1 >= 0.0;
if(m_front)
{
m_normal.setV(m_normal1);
m_lowerLimit.setV(m_normal0);
m_upperLimit.setV(m_normal1);
m_upperLimit.negativeSelf();
}
else
{
m_normal.setV(m_normal1);
m_normal.negativeSelf();
m_lowerLimit.setV(m_normal1);
m_upperLimit.setV(m_normal1);
m_upperLimit.negativeSelf();
}
}
else
{
m_front = offset0 >= 0.0 && offset1 >= 0.0;
if(m_front)
{
m_normal.setV(m_normal1);
m_lowerLimit.setV(m_normal1);
m_upperLimit.setV(m_normal1);
m_upperLimit.negativeSelf();
}
else
{
m_normal.setV(m_normal1);
m_normal.negativeSelf();
m_lowerLimit.setV(m_normal1);
m_upperLimit.setV(m_normal0);
m_upperLimit.negativeSelf();
}
}
}
else if(hasVertex3)
{
if(convex2)
{
m_front = offset1 >= 0.0 || offset2 >= 0.0;
if(m_front)
{
m_normal.setV(m_normal1);
m_lowerLimit.setV(m_normal1);
m_lowerLimit.negativeSelf();
m_upperLimit.setV(m_normal2);
}
else
{
m_normal.setV(m_normal1);
m_normal.negativeSelf();
m_lowerLimit.setV(m_normal1);
m_lowerLimit.negativeSelf();
m_upperLimit.setV(m_normal1);
}
}
else
{
m_front = offset1 >= 0.0 && offset2 >= 0.0;
if(m_front)
{
m_normal.setV(m_normal1);
m_lowerLimit.setV(m_normal1);
m_lowerLimit.negativeSelf();
m_upperLimit.setV(m_normal1);
}
else
{
m_normal.setV(m_normal1);
m_normal.negativeSelf();
m_lowerLimit.setV(m_normal2);
m_lowerLimit.negativeSelf();
m_upperLimit.setV(m_normal1);
}
}
}
else
{
m_front = offset1 >= 0.0;
if(m_front)
{
m_normal.setV(m_normal1);
m_lowerLimit.setV(m_normal1);
m_lowerLimit.negativeSelf();
m_upperLimit.setV(m_normal1);
m_upperLimit.negativeSelf();
}
else
{
m_normal.setV(m_normal1);
m_normal.negativeSelf();
m_lowerLimit.setV(m_normal1);
m_upperLimit.setV(m_normal1);
}
}
//-----------------------------
//Get polygonB in frameA
m_polygonB.count = polygonB.m_vertexCount;
//TODO: Suspicious?
for(i in 0...polygonB.m_vertexCount)
{
multiplyTransformVector(m_xf, polygonB.m_vertices[i], temp);
m_polygonB.vertices[i].setV(temp);
multiplyRotationVector(m_xf.R, polygonB.m_normals[i], temp);
m_polygonB.normals[i].setV(temp);
}
//-----------------------------
m_radius = 2.0 * B2Settings.b2_polygonRadius;
manifold.m_pointCount = 0;
computeEdgeSeparation(edgeAxis);
//If no valid normal can be found than this edge should not collide.
if(edgeAxis.type == Type.UNKNOWN)
{
return;
}
if(edgeAxis.separation > m_radius)
{
return;
}
computePolygonSeparation(polygonAxis);
if(polygonAxis.type != Type.UNKNOWN && polygonAxis.separation > m_radius)
{
trace("polysep: " + polygonAxis.separation + " is > " + m_radius);
return;
}
//Never gets past this point
trace("pass!");
//-----------------------------
//Use hysteresis for jitter reduction.
var k_relativeTol = 0.98;
var k_absoluteTol = 0.001;
var primaryAxis:EPAxis;
if(polygonAxis.type == Type.UNKNOWN)
{
primaryAxis = edgeAxis;
}
else if(polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol)
{
primaryAxis = polygonAxis;
}
else
{
primaryAxis = edgeAxis;
}
//-----------------------------
//ClipVertex[] ie = new ClipVertex[2];
if(primaryAxis.type == Type.EDGE_A)
{
manifold.m_type = B2Manifold.e_faceA;
//Search for the polygon normal that is most anti-parallel to the edge normal.
var bestIndex:Int = 0;
var bestValue:Float = B2Math.dot(m_normal, m_polygonB.normals[0]);
for(i in 1...m_polygonB.count)
{
var value:Float = B2Math.dot(m_normal, m_polygonB.normals[i]);
if(value < bestValue)
{
bestValue = value;
bestIndex = i;
}
}
var i1:Int = bestIndex;
var i2:Int = i1 + 1 < m_polygonB.count ? i1 + 1 : 0;
ie[0].v.setV(m_polygonB.vertices[i1]);
ie[0].id.indexA = 0;
ie[0].id.indexB = i1;
ie[0].id.typeA = B2ContactID.FACE;
ie[0].id.typeB = B2ContactID.VERTEX;
ie[1].v.setV(m_polygonB.vertices[i2]);
ie[1].id.indexA = 0;
ie[1].id.indexB = i2;
ie[1].id.typeA = B2ContactID.FACE;
ie[1].id.typeB = B2ContactID.VERTEX;
if(m_front)
{
rf.i1 = 0;
rf.i2 = 1;
rf.v1.setV(m_v1);
rf.v2.setV(m_v2);
rf.normal.setV(m_normal1);
}
else
{
rf.i1 = 1;
rf.i2 = 0;
rf.v1.setV(m_v2);
rf.v2.setV(m_v1);
rf.normal.setV(m_normal1);
rf.normal.negativeSelf();
}
}
else
{
manifold.m_type = B2Manifold.e_faceB;
ie[0].v.setV(m_v1);
ie[0].id.indexA = 0;
ie[0].id.indexB = primaryAxis.index;
ie[0].id.typeA = B2ContactID.VERTEX;
ie[0].id.typeB = B2ContactID.FACE;
ie[1].v.setV(m_v2);
ie[1].id.indexA = 0;
ie[1].id.indexB = primaryAxis.index;
ie[1].id.typeA = B2ContactID.VERTEX;
ie[1].id.typeB = B2ContactID.FACE;
rf.i1 = primaryAxis.index;
rf.i2 = rf.i1 + 1 < m_polygonB.count ? rf.i1 + 1 : 0;
rf.v1.setV(m_polygonB.vertices[rf.i1]);
rf.v2.setV(m_polygonB.vertices[rf.i2]);
rf.normal.setV(m_polygonB.normals[rf.i1]);
}
//-----------------------------
rf.sideNormal1.set(rf.normal.y, -rf.normal.x);
rf.sideNormal2.setV(rf.sideNormal1);
rf.sideNormal2.negativeSelf();
rf.sideOffset1 = B2Math.dot(rf.sideNormal1, rf.v1);
rf.sideOffset2 = B2Math.dot(rf.sideNormal2, rf.v2);
//Clip incident edge against extruded edge1 side edges.
var np:Int;
//Clip to box side 1
np = clipSegmentToLine(clipPoints1, ie, rf.sideNormal1, rf.sideOffset1, rf.i1);
if(np < B2Settings.b2_maxManifoldPoints)
{
return;
}
//Clip to negative box side 1
np = clipSegmentToLine(clipPoints2, clipPoints1, rf.sideNormal2, rf.sideOffset2, rf.i2);
if(np < B2Settings.b2_maxManifoldPoints)
{
return;
}
//Now clipPoints2 contains the clipped points.
if(primaryAxis.type == Type.EDGE_A)
{
manifold.m_localPlaneNormal.setV(rf.normal);
manifold.m_localPoint.setV(rf.v1);
}
else
{
manifold.m_localPlaneNormal.setV(polygonB.m_normals[rf.i1]);
manifold.m_localPoint.setV(polygonB.m_vertices[rf.i1]);
}
//-----------------------------
var pointCount:Int = 0;
for(i in 0...B2Settings.b2_maxManifoldPoints)
{
temp.setV(clipPoints2[i].v);
temp.subtract(rf.v1);
var separation:Float = B2Math.dot(rf.normal, temp);
if(separation <= m_radius)
{
var cp:B2ManifoldPoint = manifold.m_points[pointCount];
if(primaryAxis.type == Type.EDGE_A)
{
//TODO: This is definitely wrong but we never get this far anyways.
cp.m_localPoint = B2Math.mulXT(m_xf, clipPoints2[i].v);
cp.m_id.set(clipPoints2[i].id);
}
else
{
cp.m_localPoint.setV(clipPoints2[i].v);
cp.m_id.typeA = clipPoints2[i].id.typeB;
cp.m_id.typeB = clipPoints2[i].id.typeA;
cp.m_id.indexA = clipPoints2[i].id.indexB;
cp.m_id.indexB = clipPoints2[i].id.indexA;
}
pointCount++;
}
}
manifold.m_pointCount = pointCount;
}
public function computeEdgeSeparation(axis:EPAxis):Void
{
axis.type = Type.EDGE_A;
axis.index = m_front ? 0 : 1;
axis.separation = B2Math.MAX_VALUE;
for(i in 0...m_polygonB.count)
{
temp.setV(m_polygonB.vertices[i]);
temp.subtract(m_v1);
var s:Float = B2Math.dot(m_normal, temp);
if(s < axis.separation)
{
axis.separation = s;
}
}
}
public function computePolygonSeparation(axis:EPAxis):Void
{
axis.type = Type.UNKNOWN;
axis.index = -1;
axis.separation = -B2Math.MAX_VALUE;
perp.set(-m_normal.y, m_normal.x);
for(i in 0...m_polygonB.count)
{
n.setV(m_polygonB.normals[i]);
n.negativeSelf();
temp.setV(m_polygonB.vertices[i]);
temp.subtract(m_v1);
var s1:Float = B2Math.dot(n, temp);
temp.setV(m_polygonB.vertices[i]);
temp.subtract(m_v2);
var s2:Float = B2Math.dot(n, temp);
var s:Float = Math.min(s1, s2);
if(s > m_radius)
{
//No collision
axis.type = Type.EDGE_B;
axis.index = i;
axis.separation = s;
//TODO: Print out values of vertices / normals to make sure they're sane.
//trace("#" + i);
//trace("No collision: " + s + " > " + m_radius);
//trace("pt: " + m_polygonB.vertices[i].x + " , " + m_polygonB.vertices[i].y);
//trace("sep: " + s1 + " , " + s2);
return;
}
//Adjacency
if(B2Math.dot(n, perp) >= 0.0)
{
temp.setV(n);
temp.subtract(m_upperLimit);
if(B2Math.dot(temp, m_normal) < -B2Settings.b2_angularSlop)
{
continue;
}
}
else
{
temp.setV(n);
temp.subtract(m_lowerLimit);
if(B2Math.dot(temp, m_normal) < -B2Settings.b2_angularSlop)
{
continue;
}
}
if(s > axis.separation)
{
axis.type = Type.EDGE_B;
axis.index = i;
axis.separation = s;
}
}
}
public static function clipSegmentToLine
(
vOut:Array<ClipVertex>,
vIn:Array<ClipVertex>,
normal:B2Vec2,
offset:Float,
vertexIndexA:Int
):Int
{
//Start with no output points
var numOut:Int = 0;
//Calculate the distance of end points to the line
var distance0:Float = B2Math.dot(normal, vIn[0].v) - offset;
var distance1:Float = B2Math.dot(normal, vIn[1].v) - offset;
//If the points are behind the plane
if(distance0 <= 0.0)
{
vOut[numOut++].set(vIn[0]);
}
if(distance1 <= 0.0)
{
vOut[numOut++].set(vIn[1]);
}
//If the points are on different sides of the plane
if(distance0 * distance1 < 0.0)
{
//Find intersection point of edge and plane
var interp:Float = distance0 / (distance0 - distance1);
//vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v);
vOut[numOut].v.setV(vIn[1].v);
vOut[numOut].v.subtract(vIn[0].v);
vOut[numOut].v.multiply(interp);
vOut[numOut].v.add(vIn[0].v);
//VertexA is hitting edgeB.
vOut[numOut].id.indexA = vertexIndexA;
vOut[numOut].id.indexB = vIn[0].id.indexB;
vOut[numOut].id.typeA = B2ContactID.VERTEX;
vOut[numOut].id.typeB = B2ContactID.FACE;
numOut++;
}
return numOut;
}
public function multiplyTransformsInverse(A:B2Transform, B:B2Transform, out:B2Transform):Void
{
//b2MulT(A.q, B.q); Rotation * Rotation
var q = new B2Mat22();
multiplyRotationsInverse(A.R, B.R, q);
//b2MulT(A.q, B.p - A.p); Rotation * Vector
var temp1 = new B2Vec2();
var temp2 = new B2Vec2();
temp2.setV(B.position);
temp2.subtract(A.position);
multiplyRotationVectorInverse(A.R, temp2, temp1);
out.position = temp1;
out.R = q;
}
public function multiplyRotationsInverse(q:B2Mat22, r:B2Mat22, out:B2Mat22)
{
out.col1.y = q.col1.x * r.col1.y - q.col1.y * r.col1.x;
out.col1.x = q.col1.x * r.col1.x + q.col1.y * r.col1.y;
}
private function multiplyRotationVector(q:B2Mat22, v:B2Vec2, out:B2Vec2):Void
{
out.x = q.col1.x * v.x - q.col1.y * v.y;
out.y = q.col1.y * v.x + q.col1.x * v.y;
}
private function multiplyRotationVectorInverse(q:B2Mat22, v:B2Vec2, out:B2Vec2):Void
{
out.x = q.col1.x * v.x + q.col1.y * v.y;
out.y = -q.col1.y * v.x + q.col1.x * v.y;
}
private function multiplyTransformVector(T:B2Transform, v:B2Vec2, out:B2Vec2):Void
{
out.x = (T.R.col1.x * v.x - T.R.col1.y * v.y) + T.position.x;
out.y = (T.R.col1.y * v.x + T.R.col1.x * v.y) + T.position.y;
}
}
class TempPolygon
{
public var vertices:Array<B2Vec2>;
public var normals:Array<B2Vec2>;
public var count:Int;
public function new()
{
vertices = new Array<B2Vec2>();
normals = new Array<B2Vec2>();
for(i in 0...32)
{
vertices.push(new B2Vec2());
normals.push(new B2Vec2());
}
}
}
enum Type
{
UNKNOWN;
EDGE_A;
EDGE_B;
}
class EPAxis
{
public var type:Type;
public var index:Int;
public var separation:Float;
public function new()
{
}
}
class ClipVertex
{
public var v:B2Vec2;
public var id:B2ContactID;
public function new()
{
v = new B2Vec2();
id = new B2ContactID();
}
public function set(cv:ClipVertex)
{
v.setV(cv.v);
id.set(cv.id);
}
}
class ReferenceFace
{
public var i1:Int;
public var i2:Int;
public var v1:B2Vec2;
public var v2:B2Vec2;
public var normal:B2Vec2;
public var sideNormal1:B2Vec2;
public var sideNormal2:B2Vec2;
public var sideOffset1:Float;
public var sideOffset2:Float;
public function new()
{
v1 = new B2Vec2();
v2 = new B2Vec2();
normal = new B2Vec2();
sideNormal1 = new B2Vec2();
sideNormal2 = new B2Vec2();
}
}
Posted on December 08, 2012 at 11:33 AM
|
