def getSize(fromPose, toPose):
diffPose = Pose()
diffPose.x = toPose.x - fromPose.x
diffPose.y = toPose.y - fromPose.y
return math.hypot(diffPose.x, diffPose.y)
def getAngle(fromPose, toPose):
diffPose = Pose()
diffPose.x = toPose.x - fromPose.x
diffPose.y = toPose.y - fromPose.y
return math.atan2(diffPose.y, diffPose.x)
def invertedTransform(self, pose):
c_point = pose.x + pose.y * 1.0j
c_output = c_point * self._c_rotate + self._c_center
output = Pose()
output.x = c_output.real
output.y = c_output.imag
return output
def transform(self, pose):
c_point = pose.x + pose.y * 1.0j
c_output = (c_point - self._c_center) * numpy.conj(self._c_rotate)
output = Pose()
output.x = c_output.real
output.y = c_output.imag
return output
def _update_keep_x(self):
target_pose = WorldModel.get_pose(self._target)
if target_pose is None:
return False
keep_pose = Pose(self._keep_x, target_pose.y, 0.0)
keep_pose.y = tool.limit(keep_pose.y, self._range_y[0],
self._range_y[1])
angle = tool.getAngle(keep_pose, target_pose)
self.pose = Pose(keep_pose.x, keep_pose.y, angle)
return True
def get_intersection(pose1, pose2, pose3, pose4):
# get intersection of line1(pose1, pose2) and line2(pose3, pose4)
# reference:http://imagingsolution.blog107.fc2.com/blog-entry-137.html
s1 = ((pose4.x - pose3.x) * (pose1.y - pose3.y) \
- (pose4.y - pose3.y) * (pose1.x - pose3.x)) / 2.0
s2 = ((pose4.x - pose3.x) * (pose3.y - pose2.y) \
- (pose4.y - pose3.y) * (pose3.x - pose2.x)) / 2.0
coefficient = s1 / (s1 + s2)
output = Pose(0, 0, 0)
output.x = pose1.x + (pose2.x - pose1.x) * coefficient
output.y = pose1.y + (pose2.y - pose1.y) * coefficient
return output
def getConjugate(pose):
output = Pose()
output.x = pose.x
output.y = -pose.y
return output
