double manhattanHeuristic(const Transform3D &start, const Transform3D &goal) { MPQuaternion sRot = start.getRotation(); MPQuaternion tRot = goal.getRotation(); MPVec3 difference = MPVec3Subtract(start.getPosition(), goal.getPosition()); float pitchDiff = sRot.x - tRot.x; float yawDiff = sRot.y - tRot.y; float rollDiff = sRot.z - tRot.z; return std::abs(difference.x) + std::abs(difference.y) + std::abs(difference.z) + std::abs(pitchDiff) + std::abs(yawDiff) + std::abs(rollDiff); }
bool operator==(const Transform3D &lhs, const Transform3D &rhs) { MPVec3 leftPos = lhs.getPosition(); MPVec3 rightPos = rhs.getPosition(); MPQuaternion leftRot = lhs.getRotation(); MPQuaternion rightRot = rhs.getRotation(); // Only consider the (x, y, z) projection of the transform // Also, assume that the coordinates are integer-valued return (((int)leftPos.x == (int)rightPos.x) && ((int)leftPos.y == (int)rightPos.y) && ((int)leftPos.z == (int)rightPos.z) && // We store the pitch/yaw/roll as rotation xyz ((int)leftRot.x == (int)rightRot.x) && ((int)leftRot.y == (int)rightRot.y) && ((int)leftRot.z == (int)rightRot.z)); }
void Environment3D::worldToPlanner(Transform3D &state) const { MPVec3 pPos = state.getPosition(); this->worldToPlanner(pPos); MPQuaternion pRot = state.getRotation(); this->worldToPlanner(pRot); state.setPosition(pPos); state.setRotation(pRot); }
void Environment3D::plannerToWorld(Transform3D &state) const { MPVec3 wPos = state.getPosition(); this->plannerToWorld(wPos); MPQuaternion wRot = state.getRotation(); this->plannerToWorld(wRot); state.setPosition(wPos); state.setRotation(wRot); }
int transform3DHash(const Transform3D &t) { const int p1 = 73856093; const int p2 = 19349663; const int p3 = 83492791; const int p4 = 3331333; const int p5 = 393919; const int p6 = 39916801; MPVec3 pos = t.getPosition(); MPQuaternion rot = t.getRotation(); // Assume that the coordinates are integer-valued int x = (int)pos.x; int y = (int)pos.y; int z = (int)pos.z; int pitch = (int)rot.x; int yaw = (int)rot.y; int roll = (int)rot.z; return ((x*p1) ^ (y*p2) ^ (z*p3) ^ (pitch*p4) ^ (yaw*p5) ^ (roll*p6)); }