def __dist__(self):
polygonA1 = self.box.shape
polygonA2 = self.box2.shape
polygonATransform = self.body.transform
polygonB = None
polygonBTransform = None
for b in self.world_obj.world.bodies:
if b.userData == "decoration_body":
polygonB = b.fixtures[0].shape
polygonBTransform = b.transform
self.dist1 = Box2D.b2Distance(shapeA=polygonA1, shapeB=polygonB,
transformA=polygonATransform, transformB=polygonBTransform).distance
self.dist2 = Box2D.b2Distance(shapeA=polygonA2, shapeB=polygonB,
transformA=polygonATransform, transformB=polygonBTransform).distance
def __dist__(self):
polygonA1 = self.box.shape
polygonA2 = self.box2.shape
polygonATransform = self.body.transform
polygonB = None
polygonBTransform = None
for b in self.world_obj.world.bodies:
if b.userData == "decoration_body":
polygonB = b.fixtures[0].shape
polygonBTransform = b.transform
self.dist1 = Box2D.b2Distance(shapeA=polygonA1, shapeB=polygonB,
transformA=polygonATransform, transformB=polygonBTransform).distance
self.dist2 = Box2D.b2Distance(shapeA=polygonA2, shapeB=polygonB,
transformA=polygonATransform, transformB=polygonBTransform).distance
def makeping(self, rname, rnd):
tank = self.tanks[rname]
body = tank.turret
segmentLength = 65.0
blocalpos = box2d.b2Vec2(1.12, 0)
segment = box2d.b2Segment()
laserStart = (1.12, 0)
laserDir = (segmentLength, 0.0)
segment.p1 = body.GetWorldPoint(laserStart)
segment.p2 = body.GetWorldVector(laserDir)
segment.p2+=segment.p1
lambda_, normal, shape = self.w.RaycastOne(segment, False, None)
hitp = (1 - lambda_) * segment.p1 + lambda_ * segment.p2
angle = tank.get_turretangle()
dist = box2d.b2Distance(segment.p1, hitp)
if shape is not None:
hitbody = shape.GetBody()
kind = hitbody.userData['kind']
if kind == 'tank':
actor = hitbody.userData['actor']
if actor._pinged != rnd - 1:
actor._pinged = rnd
return kind, angle, dist
else:
return 'w', angle, 0
def makeping(self, rname, rnd):
robot = self.robots[rname]
body = robot.turret
segmentLength = 65.0
blocalpos = box2d.b2Vec2(1.12, 0)
segment = box2d.b2Segment()
laserStart = (1.12, 0)
laserDir = (segmentLength, 0.0)
segment.p1 = body.GetWorldPoint(laserStart)
segment.p2 = body.GetWorldVector(laserDir)
segment.p2+=segment.p1
lambda_, normal, shape = self.w.RaycastOne(segment, False, None)
hitp = (1 - lambda_) * segment.p1 + lambda_ * segment.p2
angle = robot.get_turretangle()
dist = box2d.b2Distance(segment.p1, hitp)
if shape is not None:
hitbody = shape.GetBody()
kind = hitbody.userData['kind']
if kind == 'robot':
actor = hitbody.userData['actor']
if actor._pinged != rnd - 1:
actor._pinged = rnd
return kind, angle, dist
else:
# Not sure why shape returns None here. Seems to be when the
# robot is pressed right up against a wall, though.
return 'w', angle, 0
def get_nearest_dist_obs(self):
dist = None
position = None
for i in range(self.n_obs):
distanceInput = b2.b2DistanceInput()
distanceInput.transformA = self.body.transform
distanceInput.transformB = self.obstacle[i].transform
# TODO: Show how multi polygon to one shape instance
distanceInput.proxyA = b2.b2DistanceProxy(self.body_shape[0])
distanceInput.proxyB = b2.b2DistanceProxy(
self.obstacle_shape[i][0])
distanceInput.useRadii = True
distanceOutput = b2.b2Distance(distanceInput)
if dist is None or distanceOutput.distance < dist:
position = distanceOutput.pointB
dist = distanceOutput.distance
"""
# euclidian_dist == distanceOutput.distance
euclidian_dist = np.sqrt((self.body.position[0] - distanceOutput.pointB[0]) ** 2 + \
(self.body.position[1] - distanceOutput.pointB[1]) ** 2)
# gradient of euclidian_dist. For simplific, I power 2 the distance.
grad = np.append(np.array(self.body.position), [0])
return np.array([0.5 * euclidian_dist ** 2])
"""
return np.append(np.array(position), [0])
def makeping(self, rname, rnd):
robot = self.robots[rname]
body = robot.turret
segmentLength = 65.0
blocalpos = box2d.b2Vec2(1.12, 0)
segment = box2d.b2Segment()
laserStart = (1.12, 0)
laserDir = (segmentLength, 0.0)
segment.p1 = body.GetWorldPoint(laserStart)
segment.p2 = body.GetWorldVector(laserDir)
segment.p2 += segment.p1
lambda_, normal, shape = self.w.RaycastOne(segment, False, None)
hitp = (1 - lambda_) * segment.p1 + lambda_ * segment.p2
angle = robot.get_turretangle()
dist = box2d.b2Distance(segment.p1, hitp)
if shape is not None:
hitbody = shape.GetBody()
kind = hitbody.userData['kind']
if kind == 'robot':
actor = hitbody.userData['actor']
if actor._pinged != rnd - 1:
actor._pinged = rnd
return kind, angle, dist
else:
# Not sure why shape returns None here. Seems to be when the
# robot is pressed right up against a wall, though.
return 'w', angle, 0
def dist_rod(self, rod_fix, rod_body): shape1 = self.fixtures[0].shape shape2 = rod_fix.shape transform1 = Box2D.b2Transform() pos1 = self.body.position angle1 = self.body.angle transform1.Set(pos1, angle1) transform2 = Box2D.b2Transform() pos2 = rod_body.position angle2 = rod_body.angle transform2.Set(pos2, angle2) pointA, pointB, distance, iterations = Box2D.b2Distance(shapeA=shape1, shapeB=shape2, transformA=transform1, transformB=transform2) # print(distance) return distance
def dist(self, other_polygon): """do not work on a polygon and a rod""" shape1 = self.fixtures[0].shape shape2 = other_polygon.fixtures[0].shape transform1 = Box2D.b2Transform() pos1 = self.body.position angle1 = self.body.angle transform1.Set(pos1, angle1) transform2 = Box2D.b2Transform() pos2 = other_polygon.body.position angle2 = other_polygon.body.angle transform2.Set(pos2, angle2) pointA, pointB, distance, iterations = Box2D.b2Distance(shapeA=shape1, shapeB=shape2, transformA=transform1, transformB=transform2) return distance
def min_geometry(self): min_dist = 1e2 for i in range(len(self.objs) - 1): for j in range(i+1, len(self.objs)): if i != j: shape1 = self.objs[i].fixtures[0].shape shape2 = self.objs[j].fixtures[0].shape transform1 = Box2D.b2Transform() pos1 = self.objs[i].body.position angle1 = self.objs[i].body.angle transform1.Set(pos1, angle1) transform2 = Box2D.b2Transform() pos2 = self.objs[j].body.position angle2 = self.objs[j].body.angle transform2.Set(pos2, angle2) pointA, pointB, distance, iterations = Box2D.b2Distance(shapeA=shape1, shapeB=shape2, transformA=transform1, transformB=transform2) if distance < min_dist: min_dist = distance return min_dist
def avg_geometry(self): total_separation = 0.0 for i in range(len(self.objs) - 1): for j in range(i+1, len(self.objs)): if i != j: shape1 = self.objs[i].fixtures[0].shape shape2 = self.objs[j].fixtures[0].shape transform1 = Box2D.b2Transform() pos1 = self.objs[i].body.position angle1 = self.objs[i].body.angle transform1.Set(pos1, angle1) transform2 = Box2D.b2Transform() pos2 = self.objs[j].body.position angle2 = self.objs[j].body.angle transform2.Set(pos2, angle2) pointA, pointB, distance, iterations = Box2D.b2Distance(shapeA=shape1, shapeB=shape2, transformA=transform1, transformB=transform2) # print(pointA, pointB, distance, iterations) total_separation += distance return total_separation * 2/(len(self.objs) * (len(self.objs) - 1))
def count_soft_threshold(self): count = 0.0 for i in range(len(self.objs)): min_dist = 1e2 for j in range(len(self.objs)): if i != j: shape1 = self.objs[i].fixtures[0].shape shape2 = self.objs[j].fixtures[0].shape transform1 = Box2D.b2Transform() pos1 = self.objs[i].body.position angle1 = self.objs[i].body.angle transform1.Set(pos1, angle1) transform2 = Box2D.b2Transform() pos2 = self.objs[j].body.position angle2 = self.objs[j].body.angle transform2.Set(pos2, angle2) pointA, pointB, distance, iterations = Box2D.b2Distance(shapeA=shape1, shapeB=shape2, transformA=transform1, transformB=transform2) if distance < min_dist: min_dist = distance # print(min_dist, (sigmoid(min_dist*10) - 0.5) * 2) count += (sigmoid(min_dist*10) - 0.5) * 2 return count
def count_threshold(self, threshold=0.3): count = 0 for i in range(len(self.objs)): isolated = True for j in range(len(self.objs)): if i != j: shape1 = self.objs[i].fixtures[0].shape shape2 = self.objs[j].fixtures[0].shape transform1 = Box2D.b2Transform() pos1 = self.objs[i].body.position angle1 = self.objs[i].body.angle transform1.Set(pos1, angle1) transform2 = Box2D.b2Transform() pos2 = self.objs[j].body.position angle2 = self.objs[j].body.angle transform2.Set(pos2, angle2) pointA, pointB, distance, iterations = Box2D.b2Distance(shapeA=shape1, shapeB=shape2, transformA=transform1, transformB=transform2) if distance < threshold: isolated = False if isolated: count += 1 return count
def distance(self, other):
dr = B2D.b2Distance(shapeA=self.shape,
shapeB=other.shape,
transformA=self.transform,
transformB=other.transform)
return Distance(*dr)
