Vector2D CustomImplicitSurface2::closestNormalLocal(
const Vector2D& otherPoint) const {
Vector2D pt = closestPointLocal(otherPoint);
Vector2D g = gradientLocal(pt);
if (g.lengthSquared() > 0.0) {
return g.normalized();
} else {
return g;
}
}
void PlanningProblem::solveInvalidGoalState()
{
// TODO : make a strategy for handling this situation
// strategy is to just displace goal point temporally to a valid point
for(uint i=0; i<stat_obstacles.size(); i++) {
Obstacle* ob = stat_obstacles[i];
if(ob == NULL) continue;
if(hasCollision(goal.goal_point, *ob)) {
Vector2D diff = goal.goal_point.getPosition().to2D() - Vector2D(ob->transform.p);
float displacement_ = agent->radius() + ob->shape->m_radius - diff.lenght();
if(displacement_ > 0)
goal.goal_point.setPosition( goal.goal_point.getPosition() +
(diff.normalized() * displacement_ * 1.1).to3D());
break;
}
}
}
Vector2D Game::getCameraPosition() {
Vector2D move = currentLevel->getPlayerObject()->getPosition() - oldCameraPosition;
if (move.length() > cam_radius) {
oldCameraPosition += move.normalized() * (move.length()-cam_radius);
}
if (oldCameraPosition.getX() + 320 > currentLevel->getWidth()) {
oldCameraPosition.setX(currentLevel->getWidth() - 320);
}
if (oldCameraPosition.getX() < 320) {
oldCameraPosition.setX(320);
}
if (oldCameraPosition.getY() + 240 > currentLevel->getHeight()) {
oldCameraPosition.setY(currentLevel->getHeight() - 240);
}
if (oldCameraPosition.getY() < 240) {
oldCameraPosition.setY(240);
}
// TODO use Display size etc
return oldCameraPosition;
}
float Vector2D::getAngle(const Vector2D &other) const
{
float abs = acos(normalized() * other.normalized());
abs = 180.0f * abs / M_PI;
return x * other.y - y * other.x >= 0.0f ? abs : -abs;
}
