bool collisionBallBarrierY(const Ball &ball, const BarrierY &barr, float &toc)
{
Vector3d normal = barr.plane().normal();
float CdotN = dot(ball.position(), normal);
//Distance to collision in direction of normal
CdotN = barr.plane().d() - CdotN + ball.radius();
//Speed of ball in direction of normal
float DdotN = dot(ball.velocity(), normal);
//Time of collision
toc = CdotN / DdotN;
if(toc < 0.0f)
return false; //Collision is behind ball
//Check barrier bounds
//Find Point of Impact
Vector3d PoI = ball.position() + ball.velocity() * toc;
Vector3d bPos = barr.plane().position();
//Check height bounds
if(PoI.y < bPos.y || PoI.y > bPos.y + barr.height())
return false;
//Check width bounds
float distSq = distanceSq(PoI, bPos);
if(distSq > (barr.radius() * barr.radius()))
return false;
return true;
}
bool areAdjacent(const Entity &a, const Entity &b) {
FBox box_a = a.boundingBox(), box_b = b.boundingBox();
if(box_a.ey() < box_b.y() || box_b.ey() < box_b.y())
return false;
return distanceSq(box_a.xz(), box_b.xz()) <= 1.0f;
}
bool areAdjacent(const Entity &a, const Entity &b) {
FBox box_a = a.boundingBox(), box_b = b.boundingBox();
if(box_a.max.y < box_b.min.y || box_b.max.y < box_b.min.y)
return false;
return distanceSq( FRect(box_a.min.xz(), box_a.max.xz()),
FRect(box_b.min.xz(), box_b.max.xz())) <= 1.0f;
}
bool intersectBallGoal(const Ball& s1, const Ball &s2)
{
//Squared Distance between the two (save sqrt)
float dist = distanceSq(s1.position(), s2.position());
float sumRadi = s1.radius() + s2.radius();
//Check dist vs. the sum of the two radi squared
if(dist < (sumRadi * sumRadi))
return true; //Intersection
return false; //No Intersection
}
void Ghost::updateEvent(double secsElapsed)
{
if (!mTarget) newTarget();
//check if we caught them
if (mTarget && distanceSq(pos(), mTarget->pos()) < 9.0) {
mTarget->remove();
Ghost* victim = new Ghost(mTarget->pos(), world());
victim->setVelocity(mTarget->velocity());
++mKillCount;
if (mKillCount % 3 == 0) new DormantGhostPortal(pos(), world());
newTarget();
}
newTargetTimer += secsElapsed;
if (newTargetTimer > 30) newTarget();
assert(mTarget || !(mPursue->isOn()));
Actor::updateEvent(secsElapsed);
}
bool intersectBallBarrierY(const Ball &ball, const BarrierY &barr, float &dist)
{
//Distance between the ball and the plane
dist = dot(barr.plane().normal(), ball.position()) - barr.plane().d();
//Test if ball is on front or back of plane
if(dist >= ball.radius() || dist <= -ball.radius())
return false;
// a^2 = h^2 - o^2
float aSq = distanceSq(barr.plane().position(), ball.position()) - (dist * dist);
//Check distance bounds
if(aSq > (barr.radius() * barr.radius()))
return false;
//Check height bounds
if(ball.position().y - ball.radius() > barr.plane().position().y + barr.height() ||
ball.position().y + ball.radius() < barr.plane().position().y)
return false;
//Ball intersects plane
return true;
}
Coord distanceSq(Point const &p, OptRect const &rect)
{
if (!rect) return std::numeric_limits<Coord>::max();
return distanceSq(p, *rect);
}
double Path::nearestPoint(Point const &_point, double from, double to, double *distance_squared) const
{
if ( from > to ) std::swap(from, to);
const Path& _path = *this;
unsigned int sz = _path.size();
if ( _path.closed() ) ++sz;
if ( from < 0 || to > sz )
{
THROW_RANGEERROR("[from,to] interval out of bounds");
}
double sif, st = modf(from, &sif);
double eif, et = modf(to, &eif);
unsigned int si = static_cast<unsigned int>(sif);
unsigned int ei = static_cast<unsigned int>(eif);
if(sz == 0) {// naked moveto
if (distance_squared != NULL)
*distance_squared = distanceSq(_point, _path.initialPoint());
return 0;
}
if ( si == sz )
{
--si;
st = 1;
}
if ( ei == sz )
{
--ei;
et = 1;
}
if ( si == ei )
{
double nearest = _path[si].nearestPoint(_point, st, et);
if (distance_squared != NULL)
*distance_squared = distanceSq(_point, _path[si].pointAt(nearest));
return si + nearest;
}
double t;
double nearest = _path[si].nearestPoint(_point, st);
unsigned int ni = si;
double dsq;
double mindistsq = distanceSq(_point, _path[si].pointAt(nearest));
for ( unsigned int i = si + 1; i < ei; ++i )
{
Rect bb = (_path[i].boundsFast());
dsq = distanceSq(_point, bb);
if ( mindistsq <= dsq ) continue;
t = _path[i].nearestPoint(_point);
dsq = distanceSq(_point, _path[i].pointAt(t));
if ( mindistsq > dsq )
{
nearest = t;
ni = i;
mindistsq = dsq;
}
}
Rect bb = (_path[ei].boundsFast());
dsq = distanceSq(_point, bb);
if ( mindistsq > dsq )
{
t = _path[ei].nearestPoint(_point, 0, et);
dsq = distanceSq(_point, _path[ei].pointAt(t));
if ( mindistsq > dsq )
{
nearest = t;
ni = ei;
mindistsq = dsq;
}
}
if (distance_squared != NULL)
*distance_squared = mindistsq;
return ni + nearest;
}
std::vector<double>
Path::allNearestPoints(Point const& _point, double from, double to) const
{
if ( from > to ) std::swap(from, to);
const Path& _path = *this;
unsigned int sz = _path.size();
if ( _path.closed() ) ++sz;
if ( from < 0 || to > sz )
{
THROW_RANGEERROR("[from,to] interval out of bounds");
}
double sif, st = modf(from, &sif);
double eif, et = modf(to, &eif);
unsigned int si = static_cast<unsigned int>(sif);
unsigned int ei = static_cast<unsigned int>(eif);
if ( si == sz )
{
--si;
st = 1;
}
if ( ei == sz )
{
--ei;
et = 1;
}
if ( si == ei )
{
std::vector<double> all_nearest =
_path[si].allNearestPoints(_point, st, et);
for ( unsigned int i = 0; i < all_nearest.size(); ++i )
{
all_nearest[i] = si + all_nearest[i];
}
return all_nearest;
}
std::vector<double> all_t;
std::vector< std::vector<double> > all_np;
all_np.push_back( _path[si].allNearestPoints(_point, st) );
std::vector<unsigned int> ni;
ni.push_back(si);
double dsq;
double mindistsq
= distanceSq( _point, _path[si].pointAt( all_np.front().front() ) );
Rect bb(Geom::Point(0,0),Geom::Point(0,0));
for ( unsigned int i = si + 1; i < ei; ++i )
{
bb = (_path[i].boundsFast());
dsq = distanceSq(_point, bb);
if ( mindistsq < dsq ) continue;
all_t = _path[i].allNearestPoints(_point);
dsq = distanceSq( _point, _path[i].pointAt( all_t.front() ) );
if ( mindistsq > dsq )
{
all_np.clear();
all_np.push_back(all_t);
ni.clear();
ni.push_back(i);
mindistsq = dsq;
}
else if ( mindistsq == dsq )
{
all_np.push_back(all_t);
ni.push_back(i);
}
}
bb = (_path[ei].boundsFast());
dsq = distanceSq(_point, bb);
if ( mindistsq >= dsq )
{
all_t = _path[ei].allNearestPoints(_point, 0, et);
dsq = distanceSq( _point, _path[ei].pointAt( all_t.front() ) );
if ( mindistsq > dsq )
{
for ( unsigned int i = 0; i < all_t.size(); ++i )
{
all_t[i] = ei + all_t[i];
}
return all_t;
}
else if ( mindistsq == dsq )
{
all_np.push_back(all_t);
ni.push_back(ei);
}
}
std::vector<double> all_nearest;
for ( unsigned int i = 0; i < all_np.size(); ++i )
{
for ( unsigned int j = 0; j < all_np[i].size(); ++j )
{
all_nearest.push_back( ni[i] + all_np[i][j] );
}
}
all_nearest.erase(std::unique(all_nearest.begin(), all_nearest.end()),
all_nearest.end());
return all_nearest;
}
std::vector<double> EllipticalArc::allNearestPoints( Point const& p, double from, double to ) const
{
std::vector<double> result;
if ( from > to ) std::swap(from, to);
if ( from < 0 || to > 1 )
{
THROW_RANGEERROR("[from,to] interval out of range");
}
if ( ( are_near(ray(X), 0) && are_near(ray(Y), 0) ) || are_near(from, to) )
{
result.push_back(from);
return result;
}
else if ( are_near(ray(X), 0) || are_near(ray(Y), 0) )
{
LineSegment seg(pointAt(from), pointAt(to));
Point np = seg.pointAt( seg.nearestPoint(p) );
if ( are_near(ray(Y), 0) )
{
if ( are_near(_rot_angle, M_PI/2)
|| are_near(_rot_angle, 3*M_PI/2) )
{
result = roots(np[Y], Y);
}
else
{
result = roots(np[X], X);
}
}
else
{
if ( are_near(_rot_angle, M_PI/2)
|| are_near(_rot_angle, 3*M_PI/2) )
{
result = roots(np[X], X);
}
else
{
result = roots(np[Y], Y);
}
}
return result;
}
else if ( are_near(ray(X), ray(Y)) )
{
Point r = p - center();
if ( are_near(r, Point(0,0)) )
{
THROW_INFINITESOLUTIONS(0);
}
// TODO: implement case r != 0
// Point np = ray(X) * unit_vector(r);
// std::vector<double> solX = roots(np[X],X);
// std::vector<double> solY = roots(np[Y],Y);
// double t;
// if ( are_near(solX[0], solY[0]) || are_near(solX[0], solY[1]))
// {
// t = solX[0];
// }
// else
// {
// t = solX[1];
// }
// if ( !(t < from || t > to) )
// {
// result.push_back(t);
// }
// else
// {
//
// }
}
// solve the equation <D(E(t),t)|E(t)-p> == 0
// that provides min and max distance points
// on the ellipse E wrt the point p
// after the substitutions:
// cos(t) = (1 - s^2) / (1 + s^2)
// sin(t) = 2t / (1 + s^2)
// where s = tan(t/2)
// we get a 4th degree equation in s
/*
* ry s^4 ((-cy + py) Cos[Phi] + (cx - px) Sin[Phi]) +
* ry ((cy - py) Cos[Phi] + (-cx + px) Sin[Phi]) +
* 2 s^3 (rx^2 - ry^2 + (-cx + px) rx Cos[Phi] + (-cy + py) rx Sin[Phi]) +
* 2 s (-rx^2 + ry^2 + (-cx + px) rx Cos[Phi] + (-cy + py) rx Sin[Phi])
*/
Point p_c = p - center();
double rx2_ry2 = (ray(X) - ray(Y)) * (ray(X) + ray(Y));
double sinrot, cosrot;
sincos(_rot_angle, sinrot, cosrot);
double expr1 = ray(X) * (p_c[X] * cosrot + p_c[Y] * sinrot);
Poly coeff;
coeff.resize(5);
coeff[4] = ray(Y) * ( p_c[Y] * cosrot - p_c[X] * sinrot );
coeff[3] = 2 * ( rx2_ry2 + expr1 );
coeff[2] = 0;
coeff[1] = 2 * ( -rx2_ry2 + expr1 );
coeff[0] = -coeff[4];
// for ( unsigned int i = 0; i < 5; ++i )
// std::cerr << "c[" << i << "] = " << coeff[i] << std::endl;
std::vector<double> real_sol;
// gsl_poly_complex_solve raises an error
// if the leading coefficient is zero
if ( are_near(coeff[4], 0) )
{
real_sol.push_back(0);
if ( !are_near(coeff[3], 0) )
{
double sq = -coeff[1] / coeff[3];
if ( sq > 0 )
{
double s = std::sqrt(sq);
real_sol.push_back(s);
real_sol.push_back(-s);
}
}
}
else
{
real_sol = solve_reals(coeff);
}
for ( unsigned int i = 0; i < real_sol.size(); ++i )
{
real_sol[i] = 2 * std::atan(real_sol[i]);
if ( real_sol[i] < 0 ) real_sol[i] += 2*M_PI;
}
// when s -> Infinity then <D(E)|E-p> -> 0 iff coeff[4] == 0
// so we add M_PI to the solutions being lim arctan(s) = PI when s->Infinity
if ( (real_sol.size() % 2) != 0 )
{
real_sol.push_back(M_PI);
}
double mindistsq1 = std::numeric_limits<double>::max();
double mindistsq2 = std::numeric_limits<double>::max();
double dsq;
unsigned int mi1, mi2;
for ( unsigned int i = 0; i < real_sol.size(); ++i )
{
dsq = distanceSq(p, pointAtAngle(real_sol[i]));
if ( mindistsq1 > dsq )
{
mindistsq2 = mindistsq1;
mi2 = mi1;
mindistsq1 = dsq;
mi1 = i;
}
else if ( mindistsq2 > dsq )
{
mindistsq2 = dsq;
mi2 = i;
}
}
double t = map_to_01( real_sol[mi1] );
if ( !(t < from || t > to) )
{
result.push_back(t);
}
bool second_sol = false;
t = map_to_01( real_sol[mi2] );
if ( real_sol.size() == 4 && !(t < from || t > to) )
{
if ( result.empty() || are_near(mindistsq1, mindistsq2) )
{
result.push_back(t);
second_sol = true;
}
}
// we need to test extreme points too
double dsq1 = distanceSq(p, pointAt(from));
double dsq2 = distanceSq(p, pointAt(to));
if ( second_sol )
{
if ( mindistsq2 > dsq1 )
{
result.clear();
result.push_back(from);
mindistsq2 = dsq1;
}
else if ( are_near(mindistsq2, dsq) )
{
result.push_back(from);
}
if ( mindistsq2 > dsq2 )
{
result.clear();
result.push_back(to);
}
else if ( are_near(mindistsq2, dsq2) )
{
result.push_back(to);
}
}
else
{
if ( result.empty() )
{
if ( are_near(dsq1, dsq2) )
{
result.push_back(from);
result.push_back(to);
}
else if ( dsq2 > dsq1 )
{
result.push_back(from);
}
else
{
result.push_back(to);
}
}
}
return result;
}
double Point2D::distance(Point2D p) {
return sqrt(distanceSq(p));
}
