Bouncing Balls in OSG

#include "StdAfx.h"
#include "Situation.h"
#include "Simulator.h"

CSituation::CSituation( CSimulator *sim, double t, osg::Vec3d s, osg::Vec3d v )
{
	this->sim = sim;
	s0 = s;
	v0 = v;
	t0 = t;
#ifdef WITH_QUAT
	rot0 = osg::Quat( 0.0, 0.0, 0.0, 1.0 );
	a0 = osg::Vec3d( 0.0, 0.0, 0.0 );
	a0L = 0.0;
#endif
}

CSituation::CSituation( const CSituation &sit )
{
	sim = sit.sim;
	s0 = sit.s0;
	v0 = sit.v0;
	t0 = sit.t0;
#ifdef WITH_QUAT
	rot0 = sit.rot0;
	a0 = sit.a0;
	a0L = sit.a0L;
#endif
}

CSituation::~CSituation(void)
{
}

void CSituation::SetBaseTime( double time )
{
	s0 = Position( time );
#ifdef WITH_QUAT
	rot0 = Rotation( time );
#endif
	t0 = time;
}

double CSituation::GetBaseTime() const
{
	return t0;
}

osg::Vec3d CSituation::GetBasePosition() const
{
	return s0;
}

osg::Vec3d CSituation::GetVelocity() const
{
	return v0;
}

osg::Vec3d CSituation::Position( double time ) const
{
	return s0 + v0 * (time-t0);
}

#ifdef WITH_QUAT
osg::Quat CSituation::Rotation( double time ) const
{
	double angular_speed = a0L;
	if( angular_speed < 1.0e-5 )
		return rot0;
	double t = time - t0;
	double half_angle = 0.5 * t * angular_speed;
	double cos_half_angle = cos( half_angle );
	double sin_half_angle = sin( half_angle );
    double factor = sin_half_angle/angular_speed;
	osg::Quat rotation;
	rotation.x() = a0.x() * factor;
	rotation.y() = a0.y() * factor;
	rotation.z() = a0.z() * factor;
	rotation.w() = cos_half_angle;
	return rot0 * rotation;
}
#endif

CSimulator *CSituation::Simulator() const
{
	return sim;
}

void CSituation::IncreaseVelocity(osg::Vec3d v)
{
	v0 += v;
}

void CSituation::SetVelocity(osg::Vec3d v)
{
	v0 = v;
}

void CSituation::SetAngularVelocity(osg::Vec3d a)
{
	a0 = a;
	a0L = a0.length();
}