swiftcollisiondetector.cpp

Go to the documentation of this file.

/*
    Sheep - A Rigid Body Dynamics Engine
    Copyright (C) 2001-2004 Francois Beaune
    Contact: http://toxicengine.sourceforge.net/

    This file is part of Sheep.

    Sheep 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.

    Sheep 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 Sheep; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#include "swiftcollisiondetector.h" // include first
#include "common/math/matrix3.h"
#include "common/math/quaternion.h"
#include "common/math/vector3.h"
#include "engine/dynamics/rigidbody.h"
#include "engine/simulation/rigidobject.h"
#include "rigidmodel.h"

#include <cassert>
//#include <iostream>

using namespace sheep;

void SWIFTCollisionDetector::Insert(RigidObject *object) {
    assert(object);

    // Make sure this object has not already been inserted.
    assert(m_object_to_id.find(object) == m_object_to_id.end());

    RigidModel *model = object->GetModel();

    const int nv = model->m_vertices.size();
    const int nf = model->m_faces.size();

    SWIFT_Real *vertices = new SWIFT_Real[3 * nv];
    int *faces = new int[3 * nf];
    int index = 0;

    //!\todo Use iterators.
    for(int i = 0; i < nv; ++i) {
        const Vector3 &v = model->m_vertices[i];

        vertices[++index] = v.m_x;
        vertices[++index] = v.m_y;
        vertices[++index] = v.m_z;
    }

    index = 0;

    //!\todo Use iterators.
    for(int i = 0; i < nf; ++i) {
        const RigidModel::Face &f = model->m_faces[i];

        faces[++index] = f.m_v0;
        faces[++index] = f.m_v1;
        faces[++index] = f.m_v2;
    }

    const int copy_oid = m_model_to_id.find(object->GetModel()) == m_model_to_id.end() ?
        DEFAULT_COPY : m_model_to_id[object->GetModel()];

    int object_id = 0;  // initialized to prevent a compiler warning

    const bool result = m_scene->Add_Object(
        vertices, faces,
        nv, nf,
        object_id,      // passed by reference
        false,          //!\todo Replace by object->GetBody()->IsFixed()
        DEFAULT_ORIENTATION,
        DEFAULT_TRANSLATION,
        DEFAULT_SCALE,
        DEFAULT_BOX_SETTING,
        DEFAULT_BOX_ENLARGE_REL,
        2.0 * HULL_THICKNESS,
        DEFAULT_FACE_VALENCES,
        copy_oid);

    assert(result);

    delete [] faces;
    delete [] vertices;

    m_id_to_object[object_id] = object;
    m_object_to_id[object] = object_id;
}

void SWIFTCollisionDetector::Remove(RigidObject *object) {
    assert(object);

    // Make sure this object has been previously inserted.
    assert(m_object_to_id.find(object) != m_object_to_id.end());

    const int object_id = m_object_to_id[object];

    m_id_to_object.erase(object_id);
    m_object_to_id.erase(object);

    m_scene->Delete_Object(object_id);
}

void SWIFTCollisionDetector::RefreshTransformations() {
    for(object_to_id_map::const_iterator it = m_object_to_id.begin();
        it != m_object_to_id.end(); ++it)
    {
        const RigidBody *body = it->first->GetBody();

        SWIFT_Real orientation[9];
        SWIFT_Real translation[3];

        const Matrix3 m = Matrix3::Rotation(body->GetOrientation());

        for(int i = 0; i < 9; i++)
            orientation[i] = m[i];

        const Vector3 &p = body->GetPosition();

        translation[0] = p.m_x;
        translation[1] = p.m_y;
        translation[2] = p.m_z;

        m_scene->Set_Object_Transformation(it->second, orientation, translation);
    }

    //!\todo Remove (this is no more necessary).
    m_scene->Activate();

    m_are_disjoint = ! m_scene->Query_Contact_Determination(
        false,
        2.0 * HULL_THICKNESS,
        m_num_pairs,        // passed by reference
        &m_oids,
        &m_distances,
        NO_NEAREST_PTS,
        NO_NORMALS,
        &m_pids,
        &m_feature_types,
        &m_feature_ids);
}

void SWIFTCollisionDetector::FindContacts(ContactList *contacts) {
    assert(contacts);

    int oid_idx = 0;        // index in m_oids[] and m_feature_types[]
    int fid_idx = 0;        // index in m_feature_ids[]

    for(int i = 0; i < m_num_pairs; ++i) {
        Contact c;

        c.m_a = m_id_to_object[m_oids[oid_idx]];

        switch(m_feature_types[oid_idx++]) {
        case SWIFT_VERTEX:
            c.m_fa.m_type = Feature::Type::VERTEX;
            c.m_fa.m_id0 = m_feature_ids[fid_idx++];
            break;
        case SWIFT_EDGE:
            c.m_fa.m_type = Feature::Type::EDGE;
            c.m_fa.m_id0 = m_feature_ids[fid_idx++];
            c.m_fa.m_id1 = m_feature_ids[fid_idx++];
            break;
        case SWIFT_FACE:
            c.m_fa.m_type = Feature::Type::FACE;
            c.m_fa.m_id0 = m_feature_ids[fid_idx++];
            break;
        default:
//          std::cout << "feature_type: " << m_feature_types[oid_idx-1] << std::endl;
            assert(!"Internal failure.");
        }

        c.m_b = m_id_to_object[m_oids[oid_idx]];

        switch(m_feature_types[oid_idx++]) {
        case SWIFT_VERTEX:
            c.m_fb.m_type = Feature::Type::VERTEX;
            c.m_fb.m_id0 = m_feature_ids[fid_idx++];
            break;
        case SWIFT_EDGE:
            c.m_fb.m_type = Feature::Type::EDGE;
            c.m_fb.m_id0 = m_feature_ids[fid_idx++];
            c.m_fb.m_id1 = m_feature_ids[fid_idx++];
            break;
        case SWIFT_FACE:
            c.m_fb.m_type = Feature::Type::FACE;
            c.m_fb.m_id0 = m_feature_ids[fid_idx++];
            break;
        default:
            assert(!"Internal failure.");
        }

        // Only keep contact data for pairs of disjoint objects.
        if(m_distances[i] >= 0.0)
            contacts->push_back(c);
    }
}

---