void CCircle::AppendProperties(std::ostream & strm) const
{
	CSolidShape::AppendProperties(strm);
	strm << "\tPosition center = (" << m_positionCenter.x << ", " 
									<< m_positionCenter.y << ")" << std::endl
		<< "\tRadius = " << GetRadius() << std::endl;
}

bool RigidBody::SPHEREvsAABB(RigidBody* actor)
{


	vec3 temp_length = m_position - actor->GetPosition();

	float length = glm::length(temp_length);

	float intersection = m_radius + actor->GetWidth() - length;


	// First, compute the distance between the centers 
	glm::vec3 SepAxis = actor->GetPosition() - GetPosition();
	float Dist = glm::length(SepAxis);

	// then, find the unit vector that points from the box center to the sphere center
	glm::normalize(SepAxis);

	// divide each component of the unit vector by the maximum component, effectively "normalizing" the unit vector
	if (SepAxis.x >= SepAxis.y && SepAxis.x >= SepAxis.z)
		SepAxis /= SepAxis.x;
	else if (SepAxis.y >= SepAxis.x && SepAxis.y >= SepAxis.z)
		SepAxis /= SepAxis.y;
	else
		SepAxis /= SepAxis.z;

	// Now, find the effective radius of the box along the "normalized" unit vector pointing to the sphere
	SepAxis.x *= actor->GetWidth() / 2.0f;
	SepAxis.y *= actor->GetHeight() / 2.0f;
	SepAxis.z *= actor->GetLength() / 2.0f;

	float Dist2 = glm::length(SepAxis);

	// Finally, add the sphere radius to the box radius and compare to the distance
	if (Dist <= (GetRadius() + Dist2))
	{


		vec3 collisionNormal = glm::normalize(temp_length);
		vec3 relativeVelocity = m_velocity - actor->GetVelocity();
		vec3 collisionVector = collisionNormal *(glm::dot(relativeVelocity, collisionNormal));
		vec3 forceVector = collisionVector * 1.0f / (1 / m_mass + 1 / actor->GetMass());

		applyForceToActor(actor, 1 * forceVector); // the 1* should really be *2

		//move our spheres out of collision
		vec3 seperationVector = collisionNormal * intersection * .50f;
		m_position += seperationVector;
		actor->m_position -= seperationVector;


		std::cout << "Colliding with: CUBE!" << " ID:" << m_id << "\n";
		return true;

	}
	else
		return false;


}

const SHAPE_LINE_CHAIN SHAPE_ARC::ConvertToPolyline( double aAccuracy ) const
{
    SHAPE_LINE_CHAIN rv;
    double r = GetRadius();
    double sa = GetStartAngle();
    auto c = GetCenter();
    int n;

    if( r == 0.0 )
    {
        n = 0;
    }
    else
    {
        n = GetArcToSegmentCount( r, From_User_Unit( MILLIMETRES, aAccuracy ), m_centralAngle );
    }

    for( int i = 0; i <= n ; i++ )
    {
        double a = sa + m_centralAngle * (double) i / (double) n;
        double x = c.x + r * cos( a * M_PI / 180.0 );
        double y = c.y + r * sin( a * M_PI / 180.0 );

        rv.Append( (int) x, (int) y );
    }

    return rv;
}

bool Surfel::Contains (
    const Vec3Df&   iPoint
) const {
    float distance = ( iPoint - GetPosition () ).getLength ();

    return ( distance <= GetRadius () );
}

void    CprobeIGC::Terminate(void)
{
    AddRef();

    DataProbeTypeIGC*  dataProbeType = (DataProbeTypeIGC*)(m_probeType->GetData());

    if (dataProbeType->dtRipcord >= 0.0f)
    {
        GetMyMission()->GetIgcSite()->DestroyTeleportProbe(this);
    }

    if (m_projectileType)
    {
        m_projectileType->Release();
        m_projectileType = NULL;
    }

    m_launcher = NULL;
    m_target = NULL;

    GetCluster()->GetClusterSite()->AddExplosion(GetPosition(), GetRadius(), c_etProbe);

    GetMyMission()->DeleteProbe(this);
	TmodelIGC<IprobeIGC>::Terminate();

    if (m_probeType)
    {
        m_probeType->Release();
        m_probeType = NULL;
    }
    Release();
}

void ImgMaskCreator::DrawLine(	const std::vector<PointPair>&	pairVect, 
								const std::vector<Space>&		position,
								PelGray8						lineVal)
{
	static const double PIdiv180 = 3.1415926/180.0;

	if(m_img.IsNull())	return;

	m_position		= position;
	m_boundLineVal	= lineVal;

	m_img.Maximize();

	size_t radius	= GetRadius();
	size_t size		= pairVect.size();
	for(size_t i=0; i<size; i++)
	{
		TPoint2D<long> point0 = pairVect[i].first;
		TPoint2D<long> point1 = pairVect[i].last;

		TPoint2D<long> diff   = point1 - point0;

		if(8 * abs(diff.x()) < abs(diff.y()) || diff.Mag()<radius/3 ) 
		{
			DrawLine(point0, point1);
		}
		else
		{
			DrawArc(point0, point1, radius);
		}
	}
}

void
GetNextPoint ( gsl_matrix *pic, gsl_matrix *raw, gsl_vector *v1, 
		gint dx1, gint dy1, gint dx2, gint dy2, gint dx3, gint dy3,
		gint *x, gint *y ) {

	gsl_matrix_view view = gsl_matrix_submatrix ( raw, 
			gsl_vector_get ( v1, 0 ) - 1,
			gsl_vector_get ( v1, 1 ) - 1, 3, 3 );
	gsl_matrix_view view2 = gsl_matrix_submatrix ( pic, 
			gsl_vector_get ( v1, 0 ) - 1,
			gsl_vector_get ( v1, 1 ) - 1, 3, 3 );
	for ( int i=0; i<3; i++ ) {
		for ( int j=0; j<3; j++ ) {
			if ( gsl_matrix_get((gsl_matrix*)&view2,i,j) <= 5.0 
					|| gsl_matrix_get((gsl_matrix*)&view2,i,j) >= 250 ) {
				gsl_matrix_set ( (gsl_matrix*)&view, i, j, 0 );
			} else {
			gsl_matrix_set ( (gsl_matrix*)&view, i, j, 
					gsl_matrix_get ( (gsl_matrix*)&view, i, j )
					+ GetRadius ( i-1, j-1 ) * weight
					+ gsl_matrix_get ( (gsl_matrix*)&view2, i, j ) * huidu );
			}
		}
	}
	gfloat x0 = gsl_vector_get ( v1, 0 );
	gfloat y0 = gsl_vector_get ( v1, 1 );
	gfloat f1 = gsl_matrix_get ( raw, x0+dx1, y0+dy1 ); 
	gfloat f2 = gsl_matrix_get ( raw, x0+dx2, y0+dy2 ); 
	gfloat f3 = gsl_matrix_get ( raw, x0+dx3, y0+dy3 ); 
	gfloat f = f1; *x = x0 + dx1; *y = y0 + dy1;
	if ( f2 > f ) f = f2, *x = x0 + dx2, *y = y0 + dy2;
	if ( f3 > f ) f = f3, *x = x0 + dx3, *y = y0 + dy3;
//	if ( f1 > threshold && abs(f1 - f) < threshold ) *x = x0 + dx1, *y = y0 + dy1;
}

 ///\brief
 ///Evaluates the bounding box of the capsule
 inline void GetBoundingBox(hkvAlignedBBox &dest)
 {
   dest.m_vMin = m_vStartPosition;
   dest.m_vMax = m_vStartPosition;
   dest.expandToInclude(m_vStartPosition+m_vDirection*GetLength());
   dest.addBoundary (hkvVec3 (GetRadius()));
 }

UDWORD getComponentRadius(BASE_STATS *psComponent)
{
	iIMDShape *ComponentIMD = nullptr;
	iIMDShape *MountIMD = nullptr;
	SDWORD compID;

	compID = StatIsComponent(psComponent);
	if (compID >= 0)
	{
		StatGetComponentIMD(psComponent, compID, &ComponentIMD, &MountIMD);
		if (ComponentIMD)
		{
			return GetRadius(ComponentIMD);
		}
	}

	/* VTOL bombs are only stats allowed to have NULL ComponentIMD */
	if (StatIsComponent(psComponent) != COMP_WEAPON
	    || (((WEAPON_STATS *)psComponent)->weaponSubClass != WSC_BOMB
	        && ((WEAPON_STATS *)psComponent)->weaponSubClass != WSC_EMP))
	{
		ASSERT(ComponentIMD, "No ComponentIMD!");
	}

	return COMPONENT_RADIUS;
}

ObservationZone::Boundary
AnnularSectorZone::GetBoundary() const
{
  Boundary boundary;

  const unsigned steps = 20;
  const Angle delta = Angle::FullCircle() / steps;
  const Angle start = GetStartRadial().AsBearing();
  Angle end = GetEndRadial().AsBearing();
  if (end <= start + Angle::FullCircle() / 512)
    end += Angle::FullCircle();

  const GeoPoint inner_start =
    GeoVector(GetInnerRadius(), GetStartRadial()).EndPoint(GetReference());
  const GeoPoint inner_end =
    GeoVector(GetInnerRadius(), GetEndRadial()).EndPoint(GetReference());

  GeoVector inner_vector(GetInnerRadius(), start + delta);
  for (; inner_vector.bearing < end; inner_vector.bearing += delta)
    boundary.push_front(inner_vector.EndPoint(GetReference()));

  boundary.push_front(inner_end);
  boundary.push_front(inner_start);

  GeoVector vector(GetRadius(), start + delta);
  for (; vector.bearing < end; vector.bearing += delta)
    boundary.push_front(vector.EndPoint(GetReference()));

  boundary.push_front(GetSectorEnd());
  boundary.push_front(GetSectorStart());

  return boundary;
}

// -----------------------------------------------------------------------------
// Calculate kinematics quantities (slip angle, longitudinal slip, camber angle,
// and toe-in angle using the current state of the associated wheel body.
// -----------------------------------------------------------------------------
void ChTire::CalculateKinematics(double time, const WheelState& state, const ChTerrain& terrain) {
    // Wheel normal (expressed in global frame)
    ChVector<> wheel_normal = state.rot.GetYaxis();

    // Terrain normal at wheel location (expressed in global frame)
    ChVector<> Z_dir = terrain.GetNormal(state.pos.x(), state.pos.y());

    // Longitudinal (heading) and lateral directions, in the terrain plane
    ChVector<> X_dir = Vcross(wheel_normal, Z_dir);
    X_dir.Normalize();
    ChVector<> Y_dir = Vcross(Z_dir, X_dir);

    // Tire reference coordinate system
    ChMatrix33<> rot;
    rot.Set_A_axis(X_dir, Y_dir, Z_dir);
    ChCoordsys<> tire_csys(state.pos, rot.Get_A_quaternion());

    // Express wheel linear velocity in tire frame
    ChVector<> V = tire_csys.TransformDirectionParentToLocal(state.lin_vel);
    // Express wheel normal in tire frame
    ChVector<> n = tire_csys.TransformDirectionParentToLocal(wheel_normal);

    // Slip angle
    double abs_Vx = std::abs(V.x());
    double zero_Vx = 1e-4;
    m_slip_angle = (abs_Vx > zero_Vx) ? std::atan(V.y() / abs_Vx) : 0;

    // Longitudinal slip
    m_longitudinal_slip = (abs_Vx > zero_Vx) ? -(V.x() - state.omega * GetRadius()) / abs_Vx : 0;

    // Camber angle
    m_camber_angle = std::atan2(n.z(), n.y());
}

bool 
SectorZone::IsInSector(const GeoPoint &location) const
{
  GeoVector f(GetReference(), location);

  return f.distance <= GetRadius() && IsAngleInSector(f.bearing);
}

void DynamicObject::Update(uint32 p_time)
{
    // caster can be not in world at time dynamic object update, but dynamic object not yet deleted in Unit destructor
    Unit* caster = GetCaster();
    if (!caster)
    {
        Delete();
        return;
    }

    bool deleteThis = false;

    if (m_aliveDuration > int32(p_time))
        m_aliveDuration -= p_time;
    else
        deleteThis = true;

    if (m_effIndex < 4)
    {
        if (m_updateTimer < p_time)
        {
            Oregon::DynamicObjectUpdater notifier(*this,caster);
            VisitNearbyObject(GetRadius(), notifier);
            m_updateTimer = 500; // is this official-like?
        }else m_updateTimer -= p_time;
    }

    if (deleteThis)
    {
        caster->RemoveDynObjectWithGUID(GetGUID());
        Delete();
    }
}

bool
CylinderZone::Equals(const ObservationZonePoint &other) const
{
  const CylinderZone &z = (const CylinderZone &)other;

  return ObservationZonePoint::Equals(other) && GetRadius() == z.GetRadius();
}

bool CollisionSystem::IsColliding(std::shared_ptr<Entity> entity1,
    std::shared_ptr<Entity> entity2)
{
    auto colliderComponent1 = std::static_pointer_cast<ColliderComponent>(Engine::GetInstance().GetSingleComponentOfClass(entity1, "ColliderComponent"));
    auto colliderComponent2 = std::static_pointer_cast<ColliderComponent>(Engine::GetInstance().GetSingleComponentOfClass(entity2, "ColliderComponent"));
    auto particleComponent1 = std::static_pointer_cast<ParticleComponent>(Engine::GetInstance().GetSingleComponentOfClass(entity1, "ParticleComponent"));
    auto particleComponent2 = std::static_pointer_cast<ParticleComponent>(Engine::GetInstance().GetSingleComponentOfClass(entity2, "ParticleComponent"));
    float radius1 = colliderComponent1->GetRadius();
    float radius2 = colliderComponent2->GetRadius();
    Vector position1 = particleComponent1->GetPosition();
    Vector position2 = particleComponent2->GetPosition();
    float distance = position1.CalculateDistance(position2);

    if (distance < (radius1 + radius2))
        return true;
    return false;
}

bool Collision2D::IsWithinRadius(const sf::Sprite& sprite1, const sf::Sprite& sprite2, const float radius1)
{
	// Check for a collision
	const sf::Vector2f kfCenterSubtraction = GetCenter(sprite1) - GetCenter(sprite2);
	const float kfMagnitude = Maths2D::Magnitude(kfCenterSubtraction);
	const float kfRadiiSum = radius1 + GetRadius(sprite2);
	return kfMagnitude < kfRadiiSum;
}

void meRingCut::GetNormal( double normal[3] )
{
	GetRadius();
	for(int i=0; i<3; i++)
	{
		normal[i] = this->m_Normal[i];
	}
}

/*
*	\brief	Does this bounding box intersect with another
*/
const bool CSpaghettiBoundsSphere::Intersects( 
		CSpaghettiBounds *other,							//!< The bounding box to test against
		std::vector<CCollision> &collision
	)
{
	if (other->GetType() == BoundsType::Box)
	{
		// Sphere on box
		const bool result = other->Intersects(this, collision);
		if (!result)
			return false;

		return true;
	}
	else if (other->GetType() == BoundsType::Sphere)
	{
		// Sphere on sphere
		CSpaghettiBoundsSphere *const otherSphere = static_cast<CSpaghettiBoundsSphere*>(other);
		const float distance = (m_position - otherSphere->GetPosition()).length();
		const float radius = GetRadius() + otherSphere->GetRadius();

		if (distance <= radius)
		{
			Ogre::Vector3 collisionNormal = otherSphere->GetPosition() - GetPosition();
			collisionNormal.normalise();

			Ogre::Vector3 collisionPoint =collisionNormal * GetRadius();

			CCollision newCollision;
			newCollision.bodyOne = GetBody();
			newCollision.bodyTwo = otherSphere->GetBody();
			newCollision.collisionNormal = collisionNormal;
			newCollision.collisionPoint = GetPosition() + collisionPoint;
			newCollision.penetration = radius - distance;

			collision.push_back(newCollision);

			return true;
		}

		return false;
	}

	return false;
}

void Body::Draw(){
	if(big){
		graphics->ScaleImage(img, game->GetScaleFactor() * 1.2f);
	}else{
		graphics->ScaleImage(img, game->GetScaleFactor());
	}
	graphics->DrawImage(GetPosition(), img);
	graphics->DrawCircle(GetPosition(), GetRadius(), Color::Blue);
}

bool
AnnularSectorZone::IsInSector(const GeoPoint &location) const
{
  GeoVector f(GetReference(), location);

  return (f.distance <= GetRadius()) &&
    (f.distance >= inner_radius) &&
    IsAngleInSector(f.bearing);
}