/**
 @file World_rayCast.cpp

 Dojo Game Engine

 Copyright 2005, Morgan McGuire
 All rights reserved.
 */
#include "dojo/DApp.h"
#include "dojo/World.h"
#include "dojo/odeHelper.h"

namespace dojo {

Entity::RayCast::RayCast() : 
    ignore(NULL),
    distance(G3D::inf()),
    part(NULL),
    entity(NULL) {
}

void Entity::RayCast::odeCallback(void* instance, dGeomID ray, dGeomID geom) {
    debugAssert(instance != NULL);
    RayCast& data = *static_cast<RayCast*>(instance);

    if (dGeomGetClass(ray) != dRayClass) {
        std::swap(ray, geom);
    }
    debugAssert(dGeomGetClass(ray) == dRayClass);

    const int maxContacts = 1;
    dContact contact[maxContacts];
    int n = dCollide(ray, geom, maxContacts, &contact[0].geom, sizeof(dContact));
    if (n > 0) {
        debugAssertM(n == 1, "Rays are supposed to produce exactly one contact point!");
        if (contact[0].geom.depth < data.distance) {
            // This intersection is nearer than a previous one, investigate it

            void* gdata = dGeomGetData(geom);
            debugAssertM(gdata != NULL, "Incorrectly created Geom with no Entity.");
            Entity::Part* p = static_cast<Entity::Part*>(gdata);

            // Identify the entity and part hit
            Entity* e = p->entity;

            if (e != data.ignore) {
                // Overwrite previous contact information
                data.distance = contact[0].geom.depth;
                for (int i = 0; i < 3; ++i) {
                    data.normal[i] = static_cast<float>(contact[0].geom.normal[i]);
                }
                data.entity = e;
                data.part = p;
            }

        } // If nearest collision
    } // If collision
}


void World::rayCast(const Ray& ray, RayCastHit& hit, const EntityRef& ignore) const {
    rayCast(ray.origin, ray.direction, hit, ignore);
}


void World::rayCast(
    const Vector3&      start, 
    const Vector3&      direction,
    World::RayCastHit&  hit,
    const EntityRef&    ignore) const {

    // Let ODE handle the intersection test

    // OPT: could keep a single static Ray around instead of
    // continually creating and destroying it.
    dGeomID ray = dCreateRay(physics.spaceID, 100000);

    dGeomRaySet(
        ray, 
        start.x, start.y, start.z, 
        direction.x, direction.y, direction.z);

    Entity::RayCast data;
    if (ignore.notNull()) {
        data.ignore = const_cast<Entity*>(ignore.pointer());
    }
    // Collide the ray against the world.
    // The static cast is because dSpaceCollide2 is a C function and
    // not overloaded; see its documentation.
    dSpaceCollide2(ray, reinterpret_cast<dGeomID>(physics.spaceID), &data, Entity::RayCast::odeCallback);

    // OPT: see above
    dGeomDestroy(ray);

    // Copy result to user data structure
    hit.entity      = data.entity;
    hit.part        = data.part->name();
    hit.distance    = data.distance;
    hit.normal      = data.normal;
}

} // namespace dojo
