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)
def test_tri_bcoord (self):
tri = [ CrankBase.VecFloat2.init (5, 4),
CrankBase.VecFloat2.init (1, 2),
CrankBase.VecFloat2.init (3, 6) ]
pt = CrankBase.VecFloat2.init (3, 4)
bcoord = CrankShape.tri_bcoord (tri, pt);
self.assertFloat (bcoord.x, 0.3333);
self.assertFloat (bcoord.y, 0.3333);
self.assertFloat (bcoord.z, 0.3333);
pt = CrankBase.VecFloat2.init (5, 12)
bcoord = CrankShape.tri_bcoord (tri, pt);
self.assertFloat (bcoord.x, -0.3333);
self.assertFloat (bcoord.y, -1.3333);
self.assertFloat (bcoord.z, 2.6667);
def test_seg_intersect (self):
aa = CrankBase.VecFloat2.init (3.0, 4.0)
ab = CrankBase.VecFloat2.init (5.0, 12.0)
ba = CrankBase.VecFloat2.init (12.0, 5.0)
bb = CrankBase.VecFloat2.init (4.0, 3.0)
(res, i) = CrankShape.seg_intersect (aa, ab, ba, bb)
assert (not res)
aa = CrankBase.VecFloat2.init (2.0, 1.0)
ab = CrankBase.VecFloat2.init (5.0, 9.0)
ba = CrankBase.VecFloat2.init (1.0, 5.0)
bb = CrankBase.VecFloat2.init (4.0, 3.0)
(res, i) = CrankShape.seg_intersect (aa, ab, ba, bb)
assert (res)
self.assertFloat (i.x, 3)
self.assertFloat (i.y, (11.0 / 3.0))
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))
def test_tetra_bcoord (self):
tetra = [ CrankBase.VecFloat3.init (2, 3, 0),
CrankBase.VecFloat3.init (0, 1, 1),
CrankBase.VecFloat3.init (3, 1, 2),
CrankBase.VecFloat3.init (1, 2, 3) ]
pt = CrankBase.VecFloat3.init (2, 2, 2)
bcoord = CrankShape.tetra_bcoord (tetra, pt)
self.assertFloat (bcoord.x, 0.2667)
self.assertFloat (bcoord.y, -0.0667)
self.assertFloat (bcoord.z, 0.3333)
self.assertFloat (bcoord.w, 0.4667)
pt = CrankBase.VecFloat3.init (1, 1, 3)
bcoord = CrankShape.tetra_bcoord (tetra, pt)
self.assertFloat (bcoord.x, -0.3333)
self.assertFloat (bcoord.y, 0.3333)
self.assertFloat (bcoord.z, 0.3333)
self.assertFloat (bcoord.w, 0.6667)
def test_vec2_right (self):
a = CrankBase.VecFloat2.init (0.9950, 0.0998)
b = CrankShape.rot_vec2_right (a)
self.assertFloat (b.x, 0.0998)
self.assertFloat (b.y, -0.9950)
def test_vec2_rot (self):
a = CrankBase.VecFloat2.init (0.9950, 0.0998)
b = CrankShape.rot_vec2_rot (a, 0.5)
self.assertFloat (b.x, 0.8253)
self.assertFloat (b.y, 0.5646)
