def test_gjk (self):
a = CrankShape.Shape2CPolygon.new( [
CrankBase.VecFloat2.init (2, 1),
CrankBase.VecFloat2.init (-2, 1),
CrankBase.VecFloat2.init (-3, 0),
CrankBase.VecFloat2.init (-3, -1),
CrankBase.VecFloat2.init (-1, -2),
CrankBase.VecFloat2.init (2, -1)
] )
b = CrankShape.Shape2CPolygon.new( [
CrankBase.VecFloat2.init (2, 3),
CrankBase.VecFloat2.init (1, 3),
CrankBase.VecFloat2.init (-2, 1),
CrankBase.VecFloat2.init (-2, -1),
CrankBase.VecFloat2.init (-1, -2),
CrankBase.VecFloat2.init (0, -2),
CrankBase.VecFloat2.init (2, 0)
])
apos = CrankShape.Trans2.init ();
bpos = CrankShape.Trans2.init ();
apos.mtrans = CrankBase.VecFloat2.init (-2, 1)
bpos.mtrans = CrankBase.VecFloat2.init (3, 2)
abpos = apos.inverse().compose (bpos)
assert (not CrankShape.gjk2 (a, b, abpos))
bpos.mrot = math.pi * 7 / 6
abpos = apos.inverse().compose (bpos);
assert (not CrankShape.gjk2 (a, b, abpos))
apos.mtrans = CrankBase.VecFloat2.init (-1, 3)
bpos.mtrans = CrankBase.VecFloat2.init (0, -2)
apos.mrot = math.pi * 2 / 3
bpos.mrot = 0
apos.mscl = 0.5
bpos.mscl = 2
abpos = apos.inverse().compose (bpos);
assert (CrankShape.gjk2 (a, b, abpos))
