def test_gjk_distance (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)
self.assertFloat (CrankShape.gjk2_distance (a, b, abpos), 1.0)
bpos.mrot = math.pi * 7 / 12
abpos = apos.inverse().compose (bpos);
self.assertFloat (CrankShape.gjk2_distance (a, b, abpos), 0.0389)
apos.mtrans = CrankBase.VecFloat2.init (-5, 3)
bpos.mtrans = CrankBase.VecFloat2.init (0, -2)
apos.mrot = math.pi * 2 / 3
bpos.mrot = math.pi * 7 / 12
apos.mscl = 0.5
bpos.mscl = 2
abpos = apos.inverse().compose (bpos);
self.assertFloat (CrankShape.gjk2_distance (a, b, abpos), 1.3496)
