// Cyphesis Online RPG Server and AI Engine
// Copyright (C) 2003 Alistair Riddoch
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software Foundation,
// Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
// $Id: Collision.cpp,v 1.33 2006-10-26 00:48:07 alriddoch Exp $
#include "Collision.h"
#include "modules/Location.h"
#include "common/debug.h"
#include "common/log.h"
#include <iostream>
static const bool debug_flag = false;
////////////////////////// COLLISION //////////////////////////
bool getCollisionTime(const Point3D & p, // Position of point
const Vector3D & u, // Velocity of point
// float point_time, // Time since position set
const Point3D & l, // Position on plane
const Vector3D & n, // Plane normal
const Vector3D & v, // Velocity of plane
// float plane_time, // Time since position set
float & time) // Collision time return
//
//
// | \ n
// p ___ u | v __\ l
// \ | /
// \ | /
// \ | /
// \ | /
// \ | /
// \ | /
// _________________________\|/___________________________
//
// The time when point hits plane is as follows:
//
// ( (p + u * t) - (l + v * t) ) . n = 0
//
// dot product ( . ) is x*x + y*y + z*z
//
// (p.x + u.x * t - l.x - v.x * t) * n.x +
// (p.y + u.y * t - l.y - v.y * t) * n.y +
// (p.z + u.z * t - l.z - v.z * t) * n.z = 0
//
// p.x * n.x + u.x * n.x * t - l.x * n.x - v.x * n.x * t +
// p.y * n.y + u.y * n.y * t - l.y * n.y - v.y * n.y * t +
// p.z * n.z + u.z * n.z * t - l.z * n.z - v.z * n.z * t = 0
//
//
// ( v.x * n.x + v.y * n.y + v.z * n.z - u.x * n.x - u.y * n.y - u.z * n.z ) * t
// = ( p.x * n.x - l.x * n.x + p.y * n.y - l.y * n.y + p.z * n.z - l.z * n.z )
//
// t =
// ( p.x * n.x - l.x * n.x + p.y * n.y - l.y * n.y + p.z * n.z - l.z * n.z ) /
// ( v.x * n.x + v.y * n.y + v.z * n.z - u.x * n.x - u.y * n.y - u.z * n.z )
//
// return value should indicate whether the point was infront of the plane
// before the collision.
//
{
time = ( p.x() * n.x() - l.x() * n.x()
+ p.y() * n.y() - l.y() * n.y()
+ p.z() * n.z() - l.z() * n.z() ) /
( v.x() * n.x() + v.y() * n.y()
+ v.z() * n.z() - u.x() * n.x()
- u.y() * n.y() - u.z() * n.z() );
// Set now_infront to true if point is currently in front of the plane
bool now_infront = (Dot(p - l, n) > 0.);
// Set collided to true if the collision has alread occured
bool collided = (time < 0.);
// Return true if the collision direction is from infront,
// whether it has already happened on not
// We require logical EOR, which for bools is equivalent to !=
// return ((now_infront && !collided) || (!now_infront && collided));
return now_infront != collided;
}
// Returns true if first_collision has been updated
static
bool predictEntryExit(const CoordList & c, // Vertices of this mesh
const Vector3D & u, // Velocity of this mesh
const CoordList & o, // Vertices of other mesh
const NormalSet & n, // Normals of other mesh
const Vector3D & v, // Velocity of other mesh
float & first_collision, // Time first vertex enters
Vector3D & normal) // Returned collision normal
{
// Check l vertices against o surfaces
Vector3D entry_normal;
bool ret = false, already = false;
// Iterate over vertices
CoordList::const_iterator Iend = c.end();
for (CoordList::const_iterator I = c.begin(); I != Iend; ++I) {
debug(std::cout << "outer loop" << std::endl << std::flush;);
float last_vertex_entry = -100, first_vertex_exit = 100, time;
// Iterate over surfaces
NormalSet::const_iterator Jend = n.end();
for (NormalSet::const_iterator J = n.begin(); J != Jend; ++J) {
const Point3D & s_pos = o[J->first];
const Vector3D & s_norm = J->second;
debug(std::cout << "Testing vertex " << *I << " to surface "
<< s_pos << ": " << s_norm;);
if (getCollisionTime(*I, u, s_pos, s_norm, v, time)) {
debug(std::cout << " Collision at " << time;);
// We are colliding from infront ie entering
if (time > last_vertex_entry) {
debug(std::cout << " new";);
last_vertex_entry = time;
entry_normal = s_norm;
}
} else {
debug(std::cout << " Emergence at " << time;);
// We are colliding fron behind ie exitint
if (time < first_vertex_exit) {
first_vertex_exit = time;
}
}
debug(std::cout << std::endl << std::flush;);
}
debug(std::cout << last_vertex_entry << ":"
<< first_vertex_exit << ":"
<< first_collision << std::endl << std::flush;);
if ((last_vertex_entry < first_vertex_exit) &&
(last_vertex_entry < first_collision)) {
if (last_vertex_entry >= 0.) {
first_collision = last_vertex_entry;
debug(std::cout << "hit" << std::endl << std::flush;);
ret = true;
normal = entry_normal;
} else {
// Indicate that one or more vertices is already in collision
already = true;
}
}
}
// If one or more vertices are already in collision, and some are yet to
// collide, then we consider that the collision is immediate.
if (ret && already) {
first_collision = 0.f;
// It is possible that we need to modify the normal here, perhaps
// to cancel out velocity completely.
}
return ret;
}
bool predictCollision(const CoordList & l, // Vertices of this mesh
const NormalSet & ln, // Normals of this mesh
const Vector3D & u, // Velocity of this mesh
const CoordList & o, // Vertices of other mesh
const NormalSet & on, // Normals of other mesh
const Vector3D & v, // Velocity of other mesh
float & time, // Returned time to collision
Vector3D & n) // Returned collision normal
{
debug(std::cout << u << ", " << v << std::endl << std::flush; );
debug(std::cout << "l with o normals" << std::endl << std::flush; );
bool lo = predictEntryExit(l, u, o, on, v, time, n);
debug(std::cout << "o with l normals" << std::endl << std::flush; );
bool ol = predictEntryExit(o, v, l, ln, u, time, n);
if (ol) {
// If ol is true then the collision is in the opposite direction,
// and the normal reaction needs to be reversed.
n = -n;
}
return (lo || ol);
}
//
// This is the vertex layout used by the 3Dbox functions.
//
//
// 6
//
//
// 7 5
//
//
// 4
//
//
//
// 2
//
//
// 3 1
// z
// |
// y\ | /x 0
// \|/
bool predictCollision(const Location & l, // This location
const Location & o, // Other location
float & time, // Returned time to collision
Vector3D & normal) // Returned normal acting on l
// Predict collision between 2 entity locations
// Returns whether the collision will occur
{
// FIXME Handle entities which have no box - just one vertex I think
// FIXME THe mesh conversion process below should probably be eliminated
// by generating the data when bBox or orienation are changed.
// This would also allow us to have other mesh shapes
assert(l.bBox().isValid());
assert(o.bBox().isValid());
assert(l.velocity().isValid());
Vector3D notMoving(0., 0., 0.);
bool oMoving = o.velocity().isValid();
const Vector3D & o_velocity = oMoving ? o.velocity() : notMoving;
assert(o_velocity.isValid());
Vector3D dist = o.pos() - l.pos();
if ((dist.mag() - l.velocity().mag() * time - o_velocity.mag() * time) >
(boxBoundingRadius(l.bBox()) + boxBoundingRadius(o.bBox()))) {
return false;
}
const WFMath::Point<3> & ln = l.bBox().lowCorner();
const WFMath::Point<3> & lf = l.bBox().highCorner();
const WFMath::Point<3> & on = o.bBox().lowCorner();
const WFMath::Point<3> & of = o.bBox().highCorner();
// Create a set of vertices representing the box corners
CoordList lbox(8), obox(8);
lbox[0] = WFMath::Point<3>(ln.x(), ln.y(), ln.z());
lbox[1] = WFMath::Point<3>(lf.x(), ln.y(), ln.z());
lbox[2] = WFMath::Point<3>(lf.x(), lf.y(), ln.z());
lbox[3] = WFMath::Point<3>(ln.x(), lf.y(), ln.z());
lbox[4] = WFMath::Point<3>(ln.x(), ln.y(), lf.z());
lbox[5] = WFMath::Point<3>(lf.x(), ln.y(), lf.z());
lbox[6] = WFMath::Point<3>(lf.x(), lf.y(), lf.z());
lbox[7] = WFMath::Point<3>(ln.x(), lf.y(), lf.z());
obox[0] = WFMath::Point<3>(on.x(), on.y(), on.z());
obox[1] = WFMath::Point<3>(of.x(), on.y(), on.z());
obox[2] = WFMath::Point<3>(of.x(), of.y(), on.z());
obox[3] = WFMath::Point<3>(on.x(), of.y(), on.z());
obox[4] = WFMath::Point<3>(on.x(), on.y(), of.z());
obox[5] = WFMath::Point<3>(of.x(), on.y(), of.z());
obox[6] = WFMath::Point<3>(of.x(), of.y(), of.z());
obox[7] = WFMath::Point<3>(on.x(), of.y(), of.z());
// Set up a set of surface normals, each with an assoicated corner
NormalSet lnormals;
lnormals.insert(std::make_pair(0, Vector3D( 0., 0., -1.))); // Bottom face
lnormals.insert(std::make_pair(1, Vector3D( 0., -1., 0.))); // South face
lnormals.insert(std::make_pair(3, Vector3D(-1., 0., 0.))); // West face
lnormals.insert(std::make_pair(2, Vector3D( 1., 0., 0.))); // East face
lnormals.insert(std::make_pair(6, Vector3D( 0., 1., 0.))); // North face
lnormals.insert(std::make_pair(4, Vector3D( 0., 0., 1.))); // Top face
NormalSet onormals(lnormals);
static const Quaternion identity(1, 0, 0, 0);
// Orient the surface normals and box corners
if (l.orientation().isValid()) {
NormalSet::iterator Iend = lnormals.end();
for (NormalSet::iterator I = lnormals.begin(); I != Iend; ++I) {
I->second.rotate(l.orientation());
}
for (int i = 0; i < 8; ++i) {
lbox[i] = lbox[i].toParentCoords(l.pos(), l.orientation());
}
} else {
for (int i = 0; i < 8; ++i) {
lbox[i] = lbox[i].toParentCoords(l.pos(), identity);
}
}
if (o.orientation().isValid()) {
NormalSet::iterator Iend = onormals.end();
for (NormalSet::iterator I = onormals.begin(); I != Iend; ++I) {
I->second.rotate(o.orientation());
}
for (int i = 0; i < 8; ++i) {
obox[i] = obox[i].toParentCoords(o.pos(), o.orientation());
}
} else {
for (int i = 0; i < 8; ++i) {
obox[i] = obox[i].toParentCoords(o.pos(), identity);
}
}
#if 0
// This must be done whether orientation is valid or not
// Translate the box corners
for(int i = 0; i < 8; ++i) {
lbox[i] += l.pos();
obox[i] += o.pos();
}
#endif
// Predict the collision using the generic mesh function
return predictCollision(lbox, lnormals, l.velocity(),
obox, onormals, o_velocity,
time, normal);
}
////////////////////////// EMERGENCE //////////////////////////
bool getEmergenceTime(const Point3D & p, // Position of point
const Vector3D & u, // Velocity of point
// float point_time, // Time since position set
const Point3D & l, // Position on plane
const Vector3D & n, // Plane normal
const Vector3D & v, // Velocity of plane
// float plane_time, // Time since position set
float & time) // Emergence time return
{
return !getCollisionTime(p, u, l, n, v, time);
}
static float min(float a, float b, float c)
{
if (a < b) {
if (a < c) {
return a;
} else {
return c;
}
} else if (b < c) {
return b;
} else {
return c;
}
}
bool predictEmergence(const CoordList & l, // Vertices of this mesh
const Vector3D & u, // Velocity of this mesh
const WFMath::AxisBox<3> & o,// Bounding box of container
float & time) // Returned time to emergence
{
const WFMath::Point<3> & on = o.lowCorner();
const WFMath::Point<3> & of = o.highCorner();
float maxtime = -1;
CoordList::const_iterator Iend = l.end();
for (CoordList::const_iterator I = l.begin(); I != Iend; ++I) {
float xtime = (u.x() >= 0.f) ? ((of.x() - I->x()) / u.x())
: ((on.x() - I->x()) / u.x());
float ytime = (u.y() >= 0.f) ? ((of.y() - I->y()) / u.y())
: ((on.y() - I->y()) / u.y());
float ztime = (u.z() >= 0.f) ? ((of.z() - I->z()) / u.z())
: ((on.z() - I->z()) / u.z());
// Determine the time taken for the box corner to reach the nearest
// edge.
float ctime = min(xtime, ytime, ztime);
debug(std::cout << xtime << ":" << ytime << ":" << ztime << ":" << ctime
<< std::endl << std::flush;);
// maxtime is the time for the last corner to make contact with the
// edge
if (ctime > maxtime) {
maxtime = ctime;
}
}
if (maxtime > 0) {
time = maxtime;
} else {
time = 0;
}
return true;
}
bool predictEmergence(const Location & l, // This location
const Location & o, // Other location
float & time) // Returned time to collision
{
// We are predicting emergence of l from its parent o
// Orientation of o is irrelevant, as children and their relative
// position are also oriented, so o's bbox is axis aligned.
// In fact the only feature of o we are interested is its bbox, so
// we could drop the Location, and take the bbox instead.
// So all we need to do is get oriented points for l, and check
// when they will first leave the axis aligned bounding values
const WFMath::Point<3> & ln = l.bBox().lowCorner();
const WFMath::Point<3> & lf = l.bBox().highCorner();
CoordList lbox(8);
lbox[0] = WFMath::Point<3>(ln.x(), ln.y(), ln.z());
lbox[1] = WFMath::Point<3>(lf.x(), ln.y(), ln.z());
lbox[2] = WFMath::Point<3>(lf.x(), lf.y(), ln.z());
lbox[3] = WFMath::Point<3>(ln.x(), lf.y(), ln.z());
lbox[4] = WFMath::Point<3>(ln.x(), ln.y(), lf.z());
lbox[5] = WFMath::Point<3>(lf.x(), ln.y(), lf.z());
lbox[6] = WFMath::Point<3>(lf.x(), lf.y(), lf.z());
lbox[7] = WFMath::Point<3>(ln.x(), lf.y(), lf.z());
// Orient the box corners
if (l.orientation().isValid()) {
for(int i = 0; i < 8; ++i) {
lbox[i] = lbox[i].toParentCoords(l.pos(), l.orientation());
}
} else {
log(WARNING, "predictEmergence(): Entity has non-valid orientation.");
}
#if 0
// Translate the box corners
for(int i = 0; i < 8; ++i) {
lbox[i] += l.pos();
}
#endif
assert(l.velocity().isValid());
// We are now ready to carry out the next step
return predictEmergence(lbox, l.velocity(), o.bBox(), time);
}
syntax highlighted by Code2HTML, v. 0.9.1