//Add of Marie
bool VRWorkpieceElement::doesCollide(Vec3d position) {
//position in argument is the lowest part of the worktool and the (0,0) point of the profile
float py = this->position[1], px = this->position[0], pz = this->position[2];
float sy = this->size[1], sx = this->size[0], sz = this->size[2];
float ptooly = position[1], ptoolx = position[0], ptoolz = position[2];
float profileLength = workpiece.cuttingProfile->getLength();
if ((py + sy/2.0f > ptooly) && (py - sy/2.0f < ptooly + profileLength))
{
float maximum = workpiece.cuttingProfile->maxProfile(position, this->position, this->size);
if ((ptoolz - maximum < pz + sz/2.0f) && (ptoolz + maximum > pz - sz/2.0) &&
(ptoolx - maximum < px + sx/2.0f) && (ptoolx + maximum > px - sx/2.0))
{
float diffx = ptoolx - px, diffz = ptoolz - pz;
if (std::abs(diffx) <= sx / 2.0f) return true;
if (std::abs(diffz) <= sz / 2.0f) return true;
float sgnx = sgn(diffx), sgnz = sgn(diffz);
Vec2d vertex(px + sgnx * sx / 2.0f, pz + sgnz * sz / 2.0f);
Vec2d toolMid(ptoolx, ptoolz);
Vec2d distanceVertexTool = vertex - toolMid;
if (distanceVertexTool.length() < maximum) {
return true;
}
}
}
return false;
}
Vec2d MaiBody::resolveForce(const MaiBody &body) const
{
if (this == &body) { return Vec2d(0, 0); } // no force is here.
Vec2d r = (position_ - body.getPosition());
float r_len = r.length();
double GMm = Constants::kGaes * mass_ * body.getMass();
return Vec2d(
-GMm * (r.x / (r_len * r_len * r_len)),
-GMm * (r.y / (r_len * r_len * r_len))
);
}
friend void normalize(Vec2d& v) { v /= v.length(); }
double TileNode::Transition::getCost(const Vec2d& selfSpeed, const TileNode& thisNode)
{
static const double TURN_PENALTY = Map::NORMAL_TURN_COST;
static const double TURN_180_PENALTY = 4 * Map::NORMAL_TURN_COST;
static const double WAYPOINT_OUT_OF_ORDER_PENALTY = Map::NORMAL_TURN_COST; // TODO - increase? // it's not so good to go through incorrect waypoint
static const double ZIGZAZ_TURN_PRIZE = -Map::NORMAL_TURN_COST / 1.5; // zigzag turn is slightly better then usual turn
double cost = Map::NORMAL_TURN_COST;
AbsoluteDirection previousTurn = m_cachedParent == nullptr ? m_turnedDirection/*assume no change*/ : m_cachedParent->m_transition.m_turnedDirection;
if (m_turnedDirection != previousTurn)
cost += TURN_PENALTY;
AbsoluteDirection minDirection = std::min(m_turnedDirection, previousTurn);
AbsoluteDirection maxDirection = std::max(m_turnedDirection, previousTurn);
if ((minDirection == AbsoluteDirection::LEFT && maxDirection == AbsoluteDirection::RIGHT)
|| (minDirection == AbsoluteDirection::UP && maxDirection == AbsoluteDirection::DOWN))
{
cost += TURN_180_PENALTY;
}
TileNode* prePrevious = m_cachedParent != nullptr ? m_cachedParent->m_transition.m_cachedParent : nullptr;
bool isZigzag = m_isZigzag // already calculated
|| ( prePrevious != nullptr
&& prePrevious->m_transition.m_turnedDirection == m_turnedDirection
&& m_cachedParent->m_transition.m_turnedDirection != m_turnedDirection );
if (isZigzag)
{
cost += ZIGZAZ_TURN_PRIZE;
}
m_isZigzag = isZigzag;
if (m_cachedParent != nullptr)
m_cachedParent->m_transition.m_isZigzag = isZigzag;
if (thisNode.m_isWaypoint)
cost += WAYPOINT_OUT_OF_ORDER_PENALTY;
// speed vector may become a bonus or penalty for transitions near start
bool isMoving = selfSpeed.m_x != 0 || selfSpeed.m_y != 0;
if (m_cachedParent != nullptr && isMoving)
{
int tilesFromStart = 0;
Vec2d correctedSpeed = selfSpeed;
TileNode* previousNode = m_cachedParent->m_transition.m_cachedParent;
while (previousNode != nullptr)
{
correctedSpeed /= 2; // further from start -> less penalty
previousNode = previousNode->m_transition.m_cachedParent;
if (++tilesFromStart > 2)
{
// too far, no sense to calculate
correctedSpeed = Vec2d(0, 0);
break;
}
}
// remove "noise"
if (std::abs(correctedSpeed.m_x) < 1)
correctedSpeed.m_x = 0;
if (std::abs(correctedSpeed.m_y) < 1)
correctedSpeed.m_y = 0;
// todo - use tilesFromStart?
Vec2d turnVector = Vec2d::fromPoint(thisNode.m_pos - m_cachedParent->m_pos);
double directionPrize = Vec2d::dot(correctedSpeed, turnVector) / selfSpeed.length(); // [-1; +1]
// almost no penalty after log2(8) = 3 tiles
static const double SPEED_VECTOR_PENALTY = -Map::NORMAL_TURN_COST * 16; // negative prize * negative penalty = positive cost incerment
static const double SPEED_VECTOR_PRIZE = -Map::NORMAL_TURN_COST * 2; // positive prize * negative penalty = negative cost increment
double costIncrement = directionPrize > 0 ? directionPrize * SPEED_VECTOR_PRIZE : directionPrize * SPEED_VECTOR_PENALTY;
double newCost = cost + costIncrement;
cost = std::max(newCost, Map::MIN_TURN_COST);
}
return cost;
}