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)