CollisionPair.h

// This file is a part of the OpenSurgSim project.
// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef SURGSIM_COLLISION_COLLISIONPAIR_H
#define SURGSIM_COLLISION_COLLISIONPAIR_H

#include <list>
#include <memory>

#include "SurgSim/Collision/Representation.h"
#include "SurgSim/DataStructures/Location.h"
#include "SurgSim/Math/Vector.h"


namespace SurgSim
{
namespace Collision
{

struct Contact
{
    Contact(const CollisionDetectionType& newType,
            const double& newDepth,
            const double& newTime,
            const SurgSim::Math::Vector3d& newContact,
            const SurgSim::Math::Vector3d& newNormal,
            const std::pair<SurgSim::DataStructures::Location,
            SurgSim::DataStructures::Location>& newPenetrationPoints) :
        type(newType), depth(newDepth), time(newTime), contact(newContact),
        normal(newNormal), penetrationPoints(newPenetrationPoints), force(SurgSim::Math::Vector3d::Zero())
    {
    }
    std::shared_ptr<Contact> makeComplimentary()
    {
        auto complimentary = std::make_shared<Contact>(type, depth, time, contact,
                             -normal, std::make_pair(penetrationPoints.second, penetrationPoints.first));
        complimentary->force = -force;
        return complimentary;
    }
    bool operator==(const Contact& contact) const
    {
        return type == contact.type &&
               std::abs(time - contact.time) < 1e-8 &&
               penetrationPoints.first.isApprox(contact.penetrationPoints.first) &&
               penetrationPoints.second.isApprox(contact.penetrationPoints.second) &&
               normal.isApprox(contact.normal);
    }
    CollisionDetectionType type;                        
    double depth;                                       
    double time;                                        
    SurgSim::Math::Vector3d contact;                    
    SurgSim::Math::Vector3d normal;                     
    std::pair<SurgSim::DataStructures::Location,
        SurgSim::DataStructures::Location> penetrationPoints;   
    SurgSim::Math::Vector3d force;                      
};

class CollisionPair
{
public:
    CollisionPair();

    CollisionPair(const std::shared_ptr<Representation>& first,
                  const std::shared_ptr<Representation>& second);

    ~CollisionPair();

    void setRepresentations(const std::shared_ptr<Representation>& first,
                            const std::shared_ptr<Representation>& second);

    const std::pair<std::shared_ptr<Representation>, std::shared_ptr<Representation>>&
            getRepresentations() const;

    CollisionDetectionType getType() const;

    std::shared_ptr<Representation> getFirst() const;

    std::shared_ptr<Representation> getSecond() const;

    bool hasContacts() const;

    void addCcdContact(const double& depth,
                       const double& time,
                       const SurgSim::Math::Vector3d& contactPoint,
                       const SurgSim::Math::Vector3d& normal,
                       const std::pair<SurgSim::DataStructures::Location,
                       SurgSim::DataStructures::Location>& penetrationPoints);

    void addDcdContact(const double& depth,
                       const SurgSim::Math::Vector3d& normal,
                       const std::pair<SurgSim::DataStructures::Location,
                       SurgSim::DataStructures::Location>& penetrationPoints);

    void addContact(const std::shared_ptr<Contact>& contact);

    void updateRepresentations();

    std::list<std::shared_ptr<Contact>>& getContacts();

    void clearContacts();

    void swapRepresentations();

    bool isSwapped() const;

    bool mayIntersect() const;

private:
    std::pair<std::shared_ptr<Representation>, std::shared_ptr<Representation>> m_representations;

    CollisionDetectionType m_type;

    std::list<std::shared_ptr<Contact>> m_contacts;

    bool m_isSwapped;
};


}; // namespace Collision
}; // namespace SurgSim

template <typename charT, typename traits>
std::basic_ostream<charT, traits>& operator << (std::basic_ostream<charT, traits>& out,
        const SurgSim::Collision::Contact& contact)
{
    out << "Type: " << contact.type << std::endl;
    out << "Depth: " << contact.depth << std::endl;
    out << "Time: " << contact.time << std::endl;
    out << "Contact: " << contact.contact.transpose() << std::endl;
    out << "Normal: " << contact.normal.transpose() << std::endl;
    out << "Force: " << contact.force.transpose() << std::endl;
    out << "Penetration Point 1 :" << contact.penetrationPoints.first << std::endl;
    out << "Penetration Point 2 :" << contact.penetrationPoints.second << std::endl;
    return out;
}
#endif