/*
* Copyright (C) 2008 Jakub Filak <xfilak01@stud.fit.vutbr.cz>
*
* This file is part of rob08
*
* rob08 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 3 of the License, or
* any later version.
*
* rob08 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 rob08. If not, see <http://www.gnu.org/licenses/>.
*/
#include "world.h"
#include "geometry.h"
#include "physics.h"
#include <cmath>
#include <cassert>
#include <iostream>
#include <iomanip>
#include <limits>
#define TRILOBOT_SIZE 7
#if DEBUG_COLLISIONS
# define DEBUG_BORDER_CIRCLE
# define DEBUG_COLLTIME
#endif
using std::setw;
// /////////////////////////////////////////////////////////////////////////////
// Object sample
class SampleObject : public IMovingObject
{
public:
SampleObject( double speed, double turn, double size, const struct Position& pos )
: IObject(), IMovingObject(),
m_now( DateTime::Now() ),
m_speed( speed ),
m_turn( turn ),
m_size( size ),
m_position( pos )
{
}
SampleObject(const SampleObject& obj )
: IObject(), IMovingObject(),
m_now( DateTime::Now() ),
m_speed( ((SampleObject&)obj).Speed(m_now) ),
m_turn( ((SampleObject&)obj).Radius(m_now) ),
m_size( ((SampleObject&)obj).Size(m_now) ),
m_position( ((SampleObject&)obj).Position(m_now) )
{
}
SampleObject& operator=( const SampleObject& obj ) {
if( this != &obj ) {
this->m_size = obj.m_size;
this->m_position = obj.m_position;
this->m_speed = obj.m_speed;
this->m_turn = obj.m_turn;
}
return *this;
}
protected:
virtual struct Position ImplPosition( const DateTime& ) {
return this->m_position;
}
virtual double ImplSize( const DateTime& ) {
return this->m_size;
}
virtual double ImplSpeed(const DateTime&) {
return this->m_speed;
}
virtual double ImplRadius(const DateTime&) {
return this->m_turn;
}
virtual void ImplSerialize( std::ostream& output ) {
output << this->m_speed << std::endl
<< this->m_turn << std::endl
<< this->m_size << std::endl;
}
virtual void ImplDeserialize( std::istream& ) {
}
private:
DateTime m_now;
double m_speed;
double m_turn;
double m_size;
struct Position m_position;
};
typedef std::vector<Position> PositionVector;
typedef std::vector<Circle> CircleVector;
typedef std::vector<SampleObject> Objects;
namespace Func
{
int SPEED_SIGN = 1;
double RoundToDigits( double number, unsigned digits=5 ) {
double base = pow( 10, digits );
return round( number*base )/base;
}
Circle CircleFromPosition( Position pos, double radius ) {
Point center( radius, 0 );
Circle result( GFuncs::Transform( pos, center ), radius );
return result;
}
Circle CircleFromObject( IMovingObject& mobj ) {
DateTime now( DateTime::Now() );
return CircleFromPosition(mobj.Position(now), mobj.Radius(now));
}
}
// /////////////////////////////////////////////////////////////////////////////
// SimpleEnviroment
//
class SimpleEnviroment : public IEnviroment
{
public:
SimpleEnviroment()
: m_objects(), m_rightTop( 400, 200 ), m_borders()
{
this->m_objects.push_back( new BaseObject(20,40,30,0) );
this->m_objects.push_back( new BaseObject(10,80,80,0) );
this->m_objects.push_back( new BaseObject(30,170,90,0) );
this->m_objects.push_back( new BaseObject(15,80,180,0) );
this->m_objects.push_back( new BaseObject(30,50,100,0) );
this->m_objects.push_back( new BaseObject(5,280,160,0) );
this->m_objects.push_back( new BaseObject(25,330,80,0) );
this->m_objects.push_back( new BaseObject(20,200,150,0) );
this->m_objects.push_back( new BaseObject(5,130,60,0) );
this->m_objects.push_back( new BaseObject(10,380,20,0) );
this->m_objects.push_back( new BaseObject(30,280,45,0) );
this->m_objects.push_back( new BaseObject(35,160,120,0) );
this->m_objects.push_back( new BaseObject(10,370,180,0) );
struct Point lb( TRILOBOT_SIZE, TRILOBOT_SIZE);
struct Point lt( TRILOBOT_SIZE, m_rightTop.y-TRILOBOT_SIZE );
struct Point rb( m_rightTop.x-TRILOBOT_SIZE, TRILOBOT_SIZE );
struct Vector lbb( lb, lt );
struct Vector tb( lt, m_rightTop );
struct Vector rbb( rb, m_rightTop );
struct Vector bb( lb, rb );
m_borders.push_back( lbb );
m_borders.push_back( tb );
m_borders.push_back( rbb );
m_borders.push_back( bb );
}
virtual ~SimpleEnviroment()
{
for( unsigned i=0 ; i<this->m_objects.size() ; i++ )
{
delete this->m_objects[i];
}
}
protected:
struct Point ImplRightTop() const
{ return this->m_rightTop; }
const IObjectConstContainer& ImplObjects() const
{ return this->m_objects; }
virtual const BorderVector& ImplBorders() const {
return this->m_borders;
}
private:
IObjectContainer m_objects;
Point m_rightTop;
BorderVector m_borders;
};
// /////////////////////////////////////////////////////////////////////////////
// Engine
//
class SimpleEngine : public PhysicsEngine
{
protected:
virtual double ImplTimeToCollision( IMovingObject& mobj, IObject& sobj ) const;
virtual PointsVector ImplCollisionPoints( IMovingObject& mobj, IObject& sobj ) const;
virtual Position ImplPositionAfterTime( IMovingObject& mobj, double time ) const;
virtual Point ImplPositionAtTime( IMovingObject& mobj, const DateTime& time ) const;
virtual double ImplDistanceAfterTime( IMovingObject& mobj, double time ) const;
virtual double ImplDistanceAtTime( IMovingObject& mobj, const DateTime& time ) const;
virtual double ImplTimeToDistance( IMovingObject& mobj, double distance ) const;
virtual DateTime ImplTimeAfterDistance( IMovingObject& mobj, double distance, const DateTime& now ) const;
virtual double ImplTimeToPoint( IMovingObject& mobj, struct Point point ) const;
virtual double ImplGetCollTime( IMovingObject& obj, IMovingObject& reference, Point& collPoint ) const;
private:
void PrivCollisionPoints( IMovingObject& obj, IObject& sobj, PointsVector& pts ) const;
Position NewPosition( double radius, const Position& pos, double way ) const
{
if( .0 == way ) return pos;
double wayangle = GFuncs::RadiusToAngle(way,radius);
double na = GFuncs::DegToRad( wayangle-90 );
double ty = radius*cos( na );
double tx = radius*sin( na )+radius;
Position p( tx, ty, wayangle );
return GFuncs::Transform( pos, p);
}
Position NewPosition( const Position& pos, double way ) const
{
if( .0 == way ) return pos;
double rad = (M_PI*(90-pos.angle))/180;
double nx = way*cos( rad );
double ny = way*sin( rad );
return Position( pos.x+nx, pos.y+ny, pos.angle );
}
};
// /////////////////////////////////////////////////////////////////////////////
// World
//
struct World::Members
{
Members( IEnviroment* e, PhysicsEngine* g )
: env( e ), engine( g )
{}
~Members() { delete env; delete engine; }
IEnviroment* env;
PhysicsEngine* engine;
};
World::World()
: pimpl( new World::Members( new SimpleEnviroment(), new SimpleEngine() ) )
{}
World::~World() { delete this->pimpl; }
IEnviroment& World::Enviroment() const
{ return *this->pimpl->env; }
PhysicsEngine& World::Engine() const
{ return *this->pimpl->engine; }
double World::TimeToCollision( IMovingObject& obj ) const {
const IObjectConstContainer& objs( this->pimpl->env->Objects());
double minTime = std::numeric_limits<double>::infinity();
if( .0 == obj.Radius() ) {
#if DEBUG_COLLISIONS
std::cerr << "START :: COLLISION LINE BORDER" << std::endl;
#endif
const IEnviroment::BorderVector& bv( this->pimpl->env->Borders() );
for( unsigned i=0 ; i<bv.size() ; i++ )
{
Point result;
struct Vector objway( obj.Position() );
if( bv.at(i).Intersect( objway, result ) ) {
double time = this->pimpl->engine->TimeToPoint( obj, result );
if( time < minTime )
{
#if DEBUG_COLLISIONS
std::cerr << "> Setting min time to " << time << std::endl;
#endif
minTime = time;
}
}
}
#if DEBUG_COLLISIONS
std::cerr << "FINISH :: COLLISION LINE BORDER" << std::endl;
#endif
}
else {
#ifdef DEBUG_BORDER_CIRCLE
std::cerr << "START :: COLLISION RADIUS BORDER" << std::endl;
#endif
const IEnviroment::BorderVector& bv( this->pimpl->env->Borders() );
for( unsigned i=0 ; i<bv.size() ; i++ )
{
Circle objway( Func::CircleFromObject( obj ) );
PointsVector pts( objway.Intersects( bv.at(i) ));
for( unsigned j=0 ; j<pts.size() ; j++ )
{
double time = this->pimpl->engine->TimeToPoint(obj,pts.at(j));
if(time < minTime ) {
#ifdef DEBUG_BORDER_CIRCLE
std::cerr << "> Setting min time to " << time << " for border " << i << std::endl;
#endif
minTime = time;
}
}
}
#ifdef DEBUG_BORDER_CIRCLE
std::cerr << "FINISH :: COLLISION RADIUS BORDER" << std::endl;
#endif
}
for( unsigned i=0; i<objs.size(); i++ ) {
// Use engine to compute collision with static objects
#if DEBUG_COLLISIONS
std::cerr << "COMPUTING COLL TIME FOR OBJECT " << i << std::endl;
#endif
double toCol = this->pimpl->engine->TimeToCollision( obj, *objs[i] );
if( toCol < minTime ){ minTime = toCol; }
}
Func::SPEED_SIGN = static_cast<int>(obj.Speed());
#if DEBUG_COLLISIONS
std::cerr << "COLLISION TIME " << minTime << std::endl;
#endif
return minTime;
}
bool World::CollisionPoint( IMovingObject& obj, struct Point& result, double& time ) const {
double ttc = TimeToCollision( obj );
if( std::numeric_limits<double>::infinity() != ttc ) {
if( ttc == std::numeric_limits<double>::infinity() ) return false;
result = this->pimpl->engine->PositionAfterTime( obj, ttc );
time = ttc;
return true;
}
return false;
}
// /////////////////////////////////////////////////////////////////////////////
// SimpleEngine
/**
* Computes time to next collision with moving object and static object
* At first computes collision points with objects and then computes
* time to collision points
*/
double SimpleEngine::ImplTimeToCollision( IMovingObject& mobj, IObject& sobj ) const {
double minTime = std::numeric_limits<double>::infinity();
PointsVector pv;
PrivCollisionPoints( mobj, sobj, pv );
#if DEBUG_COLLISIONS
if ( pv.size() != 0 ) {
std::cerr << "START :: COLLISION OBJECTS ";
sobj.Position().Write( std::cerr ) << std::endl;
}
#endif
for( unsigned j=0; j<pv.size(); j++ ) {
double time = ImplTimeToPoint( mobj, pv.at(j) );
if( time < minTime ) {
#if DEBUG_COLLISIONS
std::cerr << "> Setting min time to " << time << std::endl;
#endif
minTime = time;
}
}
#if DEBUG_COLLISIONS
if ( pv.size() != 0 )
std::cerr << "FINISH :: COLLISION OBJECTS" << std::endl;
#endif
return minTime;
}
/**
* Computes collision points of moving object and static object
* At first convert moving object trajectory to geometric object and same do with static
* object and then computes intersects of this geometric objects
*/
PointsVector SimpleEngine::ImplCollisionPoints( IMovingObject& mobj, IObject& sobj ) const {
PointsVector pts;
PrivCollisionPoints( mobj, sobj, pts);
return pts;
}
void SimpleEngine::PrivCollisionPoints( IMovingObject& mobj, IObject& sobj, PointsVector& pts ) const {
if( mobj.Radius() != .0 ) {
Circle csta(sobj.Position(), sobj.Size()+mobj.Size());
PointsVector vec( Func::CircleFromObject(mobj).Intersects( csta ) );
pts.insert( pts.end(), vec.begin(), vec.end() );
}
else {
Circle csta( sobj.Position(), sobj.Size()+mobj.Size() );
Vector way( mobj.Position() );
PointsVector vec( csta.Intersects(way) );
pts.insert( pts.end(), vec.begin(), vec.end() );
}
}
/**
* Computes position after time duration
* Use speed and radius from moving object and computes position after time duration
* with simple geomtetric operation.
*/
Position SimpleEngine::ImplPositionAfterTime( IMovingObject& mobj , double duration ) const {
double radius = mobj.Radius();
double way = mobj.Speed() * duration;
if( .0 == radius )
return this->NewPosition( mobj.Position(), way );
return this->NewPosition( radius, mobj.Position(), way );
}
Point SimpleEngine::ImplPositionAtTime( IMovingObject& mobj, const DateTime& ) const {
// TODO: add arg now
return Point( mobj.Position() );
}
double SimpleEngine::ImplDistanceAfterTime( IMovingObject& obj, double duration ) const {
return fabs(obj.Speed()) * duration;
}
double SimpleEngine::ImplDistanceAtTime( IMovingObject& , const DateTime&) const {
// TODO: add arg now
return .0;
}
double SimpleEngine::ImplTimeToDistance( IMovingObject& obj, double distance ) const {
double time = distance / fabs(obj.Speed());
return time;
}
DateTime SimpleEngine::ImplTimeAfterDistance( IMovingObject& obj, double distance, const DateTime& now ) const {
double timeTo = this->TimeToDistance( obj, distance );
StampType dur = static_cast<StampType>( timeTo );
DateTime t( now + dur );
return t;
}
/**
* Computes time to point in moving object coord system
* At first compute distance between object actual position and required point and then
* computes time with general method to computing time to spent distance
*/
double SimpleEngine::ImplTimeToPoint( IMovingObject& mobj, struct Point point ) const {
double distance = .0;
int actsign = static_cast<int>( mobj.Speed() ); // in actual way direction
int sign = actsign*Func::SPEED_SIGN; // in history scope way direction
if( .0 != mobj.Radius() ) {
distance = Func::CircleFromObject(mobj).Distance( mobj.Position(), point,
actsign, sign );
}
else {
distance = mobj.Position().Distance( point, actsign );
double time = this->TimeToDistance( mobj, distance );
Point next( this->PositionAfterTime( mobj, time ) );
if( !(next == point) ) {
return std::numeric_limits<double>::infinity();
}
}
if( Func::RoundToDigits(distance) == .0 && sign < 0 ) {
return std::numeric_limits<double>::infinity();
}
return this->TimeToDistance( mobj, distance );
}
double SimpleEngine::ImplGetCollTime( IMovingObject& obj, IMovingObject& , Point& collPoint ) const
{
return TimeToPoint( obj, collPoint );
}
Back to main page
world.cpp [Download]