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));
}
double distanceHeuristic(const Transform3D &start, const Transform3D &goal)
{
return MPVec3EuclideanDistance(start.getPosition(), goal.getPosition());
}
