/*----------------------------------------------------------------------------

2D Physics Test Program - a cheesy test harness for 2D physics

by Chris Hecker for my Game Developer Magazine articles.  See my homepage
for more information.

NOTE: This is a hacked test program, not a nice example of Windows programming.
physics.cpp the only part of this you should look at!!!

This material is Copyright 1997 Chris Hecker, All Rights Reserved.
It's for you to read and learn from, not to put in your own articles
or books or on your website, etc.  Thank you.

Chris Hecker
checker@d6.com
http://www.d6.com/users/checker

*/

/*----------------------------------------------------------------------------

Interface functions between physics code and the dumb OS interface.

*/

// physics -> app

void Line( int X0, int Y0, int X1, int Y1 );
float GetTime( void );

extern int WorldWidth, WorldHeight;

// app -> physics

void Run( void );

void ToggleWorldSpring( void );
void ToggleBodySpring( void );
void ToggleGravity( void );
void ToggleDamping( void );

NAME=physics

CC  = cl -c -W4 -Zi -O2ab1gityp- -G5s -QIfdiv- -DSTRICT -DWIN32 -D_WIN32
ASM = ml -c -D_WIN32=1 -Zi -Zf -coff
LINK= link -map -pdb:none -debug:full -debugtype:cv -out:$(NAME).exe -subsystem:windows
DEF =-DDEBUG -DSTRICT
RC  = rc

OBJ  = $(NAME).obj win32.obj

LIBS = gdi32.lib user32.lib comdlg32.lib winmm.lib

.cpp.obj:
       $(CC) /Fo$*.obj /Fl$*.lst $<

.asm.obj:
       $(ASM) /Fo$*.obj /Fl $<

goal: $(NAME).exe

$(NAME).exe: $(OBJ) $(NAME).res makefile
        $(LINK) $(OBJ) $(LIBS) $(NAME).res

$(NAME).res: $(NAME).rc $(NAME).ico
        $(RC) -r $(NAME).rc

clean:
        del $(NAME).exe
        del *.lst
        del *.res
        del *.err
        del *.obj
        del *.map
        del *.sym
        del *.cod
        del *.pdb

/*----------------------------------------------------------------------------

2D Physics Test Program - a cheesy test harness for 2D physics

by Chris Hecker for my Game Developer Magazine articles.  See my homepage
for more information.

NOTE: This is a hacked test program, not a nice example of Windows programming.
physics.cpp the only part of this you should look at!!!

This material is Copyright 1997 Chris Hecker, All Rights Reserved.
It's for you to read and learn from, not to put in your own articles
or books or on your website, etc.  Thank you.

Chris Hecker
checker@d6.com
http://www.d6.com/users/checker

*/

/*----------------------------------------------------------------------------

math2d.h - The simple math library for the 2D physics demo.

10/13/96 - checker

*/

#if !defined(MATH2D_H)
#define MATH2D_H

// explicit dependencies
#include <math.h>       // for sqrt
#include <float.h>

/*----------------------------------------------------------------------------

Type and constant declarations.

*/

typedef float real;
typedef real r;         // for constants, ie. r(1), not for declaration use

real const REAL_MAX = FLT_MAX;
real const REAL_MIN = FLT_MIN;

real const PI = r(3.14159265358979323846);
real const Epsilon = r(0.00001);

struct vector_2
{
    real X, Y;

    inline vector_2( void );
    inline vector_2( real X, real Y );

    inline vector_2 &operator+=( vector_2 const &A );
    inline vector_2 &operator-=( vector_2 const &A );
};

struct matrix_2x2
{
    real aElements[2][2];

    inline matrix_2x2( void );
    inline matrix_2x2( real RotateByThisManyRadians );
};



/*----------------------------------------------------------------------------

Functions.

*/

inline real RadiansFrom( real Degrees );
inline real DegreesFrom( real Radians );

inline vector_2 operator-( vector_2 const &A, vector_2 const &B );
inline vector_2 operator+( vector_2 const &A, vector_2 const &B );
inline vector_2 operator*( real A, vector_2 const &B );
inline vector_2 operator*( vector_2 const &A, real B );
inline vector_2 operator/( vector_2 const &A, real B );

inline real DotProduct( vector_2 const &A, vector_2 const &B );

// A-perp dot B
inline real PerpDotProduct( vector_2 const &A, vector_2 const &B );

inline vector_2 GetPerpendicular( vector_2 const &A );
inline real GetLength( vector_2 const &A );
inline vector_2 GetNormal( vector_2 const &A ); // @todo need this?

inline vector_2 operator*( matrix_2x2 const &A, vector_2 const &B );

/*----------------------------------------------------------------------------

Inline function definitions.

*/

inline real RadiansFrom( real Degrees )
{
    return (Degrees * PI) / r(180);
}

inline real DegreesFrom( real Radians )
{
    return (Radians * r(180)) / PI;
}

inline vector_2 operator-( vector_2 const &A, vector_2 const &B )
{
    return vector_2(A.X-B.X,A.Y-B.Y);
}

inline vector_2 operator+( vector_2 const &A, vector_2 const &B )
{
    return vector_2(A.X+B.X,A.Y+B.Y);
}

inline vector_2 operator*( real A, vector_2 const &B )
{
    return vector_2(A*B.X,A*B.Y);
}

inline vector_2 operator*( vector_2 const &A, real B )
{
    return vector_2(B*A.X,B*A.Y);
}

inline vector_2 operator/( vector_2 const &A, real B )
{
    return vector_2(A.X/B,A.Y/B);
}

inline real DotProduct( vector_2 const &A, vector_2 const &B )
{
    return A.X*B.X + A.Y*B.Y;
}

inline real PerpDotProduct( vector_2 const &A, vector_2 const &B )
{
    return A.X*B.Y - A.Y*B.X;
}

inline vector_2 GetPerpendicular( vector_2 const &A )
{
    return vector_2(-A.Y,A.X);
}

inline real GetLength( vector_2 const &A )
{
    return r(sqrt(A.X*A.X + A.Y*A.Y));
}

inline vector_2 GetNormal( vector_2 const &A )
{
    real OneOverLength = r(1)/GetLength(A);
    return vector_2(OneOverLength*A.X,OneOverLength*A.Y);
}

inline vector_2::vector_2( void ) : X(r(0)), Y(r(0))
{
}

inline vector_2::vector_2( real X_, real Y_ ) : X(X_), Y(Y_)
{
}

inline vector_2 &vector_2::operator+=( vector_2 const &A )
{
    X += A.X;
    Y += A.Y;
    return *this;
}
inline vector_2 &vector_2::operator-=( vector_2 const &A )
{
    X -= A.X;
    Y -= A.Y;
    return *this;
}

inline vector_2 operator*( matrix_2x2 const &A, vector_2 const &B )
{
    return vector_2(A.aElements[0][0]*B.X + A.aElements[0][1]*B.Y,
                    A.aElements[1][0]*B.X + A.aElements[1][1]*B.Y);
}

inline matrix_2x2::matrix_2x2( void )
{
    aElements[0][0] = aElements[0][1] = aElements[1][0] = aElements[1][1] =r(0);
}

inline matrix_2x2::matrix_2x2( real Radians )
{
    real const CosAngle = (real)cos(Radians);
    real const SinAngle = (real)sin(Radians);

    aElements[0][0] = CosAngle; aElements[0][1] = -SinAngle;
    aElements[1][0] = SinAngle; aElements[1][1] =  CosAngle;
}

#endif

/*----------------------------------------------------------------------------

2D Physics Test Program - a cheesy test harness for 2D physics

by Chris Hecker for my Game Developer Magazine articles.  See my homepage
for more information.

NOTE: This is a hacked test program, not a nice example of Windows programming.
physics.cpp the only part of this you should look at!!!

This material is Copyright 1997 Chris Hecker, All Rights Reserved.
It's for you to read and learn from, not to put in your own articles
or books or on your website, etc.  Thank you.

Chris Hecker
checker@d6.com
http://www.d6.com/users/checker

*/

/*----------------------------------------------------------------------------

physics.cpp - This file implements the 2D physics simulator.

10/15/96 - checker

*/

#include <assert.h>

#include "physics.h"
#include "iface.h"

/*----------------------------------------------------------------------------

Globals

*/

// weird CodeWarrior bug with global initialization forces me to do this
#define WIDTH 400
#define HEIGHT 400

int WorldWidth = WIDTH;
int WorldHeight = HEIGHT;

simulation_world World(r(WIDTH),r(HEIGHT));

/*----------------------------------------------------------------------------

various forces you can add to the system

@todo need to figure out units here so these numbers mean something

*/

int WorldSpringActive = 0;      // spring goes from body 0: vertex 0 to origin
real const Kws = r(30);         // Hooke's spring constant
real const Kwd = r(5);          // damping constant
vector_2 WorldSpringAnchor(r(0),r(0));

int BodySpringActive = 0;       // spring goes from body 0 to body 1
real const Kbs = r(10);         // Hooke's spring constant
real const Kbd = r(5);          // damping constant
int Body0SpringVertexIndex = 2;
int Body1SpringVertexIndex = 0;

int GravityActive = 0;
vector_2 Gravity(r(0),r(-100));

int DampingActive = 0;
real const Kdl = r(2.5);        // linear damping factor
real const Kda = r(1400);       // angular damping factor

/*----------------------------------------------------------------------------

Run

*/

void Run( void )
{
    static real LastTime = GetTime();

    // use a fixed timestep until we implement a better integrator
    // real Time = GetTime();
    real Time = LastTime + r(0.02);

    World.Simulate(Time - LastTime);

    World.Render();

    LastTime = Time;
}

/*----------------------------------------------------------------------------

Toggles

*/

void ToggleWorldSpring( void )
{
    WorldSpringActive = WorldSpringActive ? 0 : 1;
}

void ToggleBodySpring( void )
{
    BodySpringActive = BodySpringActive ? 0 : 1;
}

void ToggleGravity( void )
{
    GravityActive = GravityActive ? 0 : 1;
}

void ToggleDamping( void )
{
    DampingActive = DampingActive ? 0 : 1;
}

/*----------------------------------------------------------------------------

utilities

*/

void InitializeBody( rigid_body &Body, real Density, real Width, real Height,
                    real CoefficientOfRestitution )
{
    real const Mass = Density * Width * Height;

    Body.CoefficientOfRestitution = CoefficientOfRestitution;

    Body.Width = Width;
    Body.Height = Height;

    Body.OneOverMass = r(1) / Mass;

    // integrate over the body to find the moment of inertia

    Body.OneOverCMMomentOfInertia = r(1) / ((Mass / r(12)) *
        (Width * Width + Height * Height));

    // 0-out non-vector quantities
    Body.aConfigurations[0].Orientation = r(0);
    Body.aConfigurations[0].AngularVelocity = r(0);
    Body.aConfigurations[0].Torque = r(0);
}

/*----------------------------------------------------------------------------

simulation_world ctor

*/

simulation_world::simulation_world( real WorldWidth_, real WorldHeight_ ) :
    WorldWidth(WorldWidth_), WorldHeight(WorldHeight_),
    SourceConfigurationIndex(0), TargetConfigurationIndex(1)
{
    // initialize bodies
    real const Density = r(0.01);
    InitializeBody(aBodies[0],Density,r(40),r(20),r(1));
    InitializeBody(aBodies[1],Density,r(50),r(10),r(1));

    aBodies[0].aConfigurations[0].CMVelocity = vector_2(r(40),r(10));
    aBodies[0].aConfigurations[0].AngularVelocity = r(PI);

    // initialize walls
    aWalls[0].Normal = vector_2(r(0),r(-1));
    aWalls[0].c = r(WorldHeight/2 - 3);

    aWalls[1].Normal = vector_2(r(0),r(1));
    aWalls[1].c = r(WorldHeight/2 - 3);

    aWalls[2].Normal = vector_2(r(-1),r(0));
    aWalls[2].c = r(WorldWidth/2 - 3);

    aWalls[3].Normal = vector_2(r(1),r(0));
    aWalls[3].c = r(WorldWidth/2 - 3);

    aWalls[4].Normal = GetNormal(vector_2(r(0.5),r(1)));
    aWalls[4].c = r(WorldWidth/2);

    // generate the wall lines
    for(int Counter = 0;Counter < NumberOfWalls;Counter++)
    {
        wall &Wall = aWalls[Counter];

        // make a big line in the direction of the wall

        vector_2 PointOnWall = -Wall.c * Wall.Normal;
        vector_2 v = GetPerpendicular(Wall.Normal);
        real t0 = -REAL_MAX;
        real t1 = REAL_MAX;

        // now clip the line to the walls

        for(int WallIndex = 0;WallIndex < NumberOfWalls;WallIndex++)
        {
            if(WallIndex != Counter)
            {
                wall &ClipWall = aWalls[WallIndex];

                real Denominator = DotProduct(v,ClipWall.Normal);

                if(fabs(Denominator) > Epsilon)
                {
                    // not coplanar

                    real t = - (ClipWall.c +
                                DotProduct(PointOnWall,ClipWall.Normal)) /
                                    Denominator;

                    if(Denominator > r(0))
                    {
                        // the clip wall's clipping the t0 side of line
                        if(t > t0)
                        {
                            t0 = t;
                        }
                    }
                    else
                    {
                        // it's clipping the t1 side
                        if(t < t1)
                        {
                            t1 = t;
                        }
                    }
                }
            }
        }

        // make sure we got clipped
        assert((t0 != -REAL_MAX) && (t1 != REAL_MAX));
        // but not completely clipped
        assert(t0 < t1);

        Wall.StartPoint = PointOnWall + t0 * v;
        Wall.EndPoint = PointOnWall + t1 * v;
    }

    // calculate initial box positions
    CalculateVertices(0);
}

/*----------------------------------------------------------------------------

simulation_world dtor

*/

simulation_world::~simulation_world( void )
{

}

/*----------------------------------------------------------------------------

Render - render the source configurations

*/

#pragma warning(disable:4244)       // float -> int
void simulation_world::Render( void )
{
    int Counter;

    // draw walls

    for(Counter = 0;Counter < NumberOfWalls;Counter++)
    {
        wall &Wall = aWalls[Counter];

        Line(Wall.StartPoint.X,Wall.StartPoint.Y,
            Wall.EndPoint.X,Wall.EndPoint.Y);
    }

    // draw bodies

    for(Counter = 0;Counter < NumberOfBodies;Counter++)
    {
        rigid_body::configuration::bounding_box &Box =
            aBodies[Counter].aConfigurations[SourceConfigurationIndex].
            BoundingBox;

        for(int Edge = 0;Edge < 4;Edge++)
        {
            int Start = Edge;
            int End = (Edge + 1) % 4;

            Line(Box.aVertices[Start].X,Box.aVertices[Start].Y,
                Box.aVertices[End].X,Box.aVertices[End].Y);
        }
    }

    // draw springs

    if(WorldSpringActive)
    {
        rigid_body::configuration::bounding_box &Box =
            aBodies[0].aConfigurations[SourceConfigurationIndex].BoundingBox;

        Line(Box.aVertices[0].X,Box.aVertices[0].Y,
            WorldSpringAnchor.X,WorldSpringAnchor.Y);
    }

    if(BodySpringActive)
    {
        rigid_body::configuration::bounding_box &Box0 =
            aBodies[0].aConfigurations[SourceConfigurationIndex].BoundingBox;
        rigid_body::configuration::bounding_box &Box1 =
            aBodies[1].aConfigurations[SourceConfigurationIndex].BoundingBox;

        assert(NumberOfBodies >= 2);
        vector_2 P0 = Box0.aVertices[Body0SpringVertexIndex];
        vector_2 P1 = Box1.aVertices[Body1SpringVertexIndex];
        Line(P0.X,P0.Y,P1.X,P1.Y);
    }

    // draw gravity vector

    if(GravityActive)
    {
        Line(0,0,0,-30);
        Line(0,-30,5,-25);
        Line(0,-30,-5,-25);
    }

    // draw damping symbol

    if(DampingActive)
    {
        Line(-5,0,5,0);
        Line(0,-5,0,5);
    }
}
#pragma warning(default:4244)       // float -> int

/*----------------------------------------------------------------------------

Simulate - Integrate forward DeltaTime seconds.

@todo do I need to store the last simulation time?

*/

void simulation_world::Simulate( real DeltaTime )
{
    real CurrentTime = r(0);
    real TargetTime = DeltaTime;

    while(CurrentTime < DeltaTime)
    {
        ComputeForces(SourceConfigurationIndex);

        Integrate(TargetTime-CurrentTime);

        CalculateVertices(TargetConfigurationIndex);

        CheckForCollisions(TargetConfigurationIndex);

        if(CollisionState == Penetrating)
        {
            // we simulated too far, so subdivide time and try again
            TargetTime = (CurrentTime + TargetTime) / r(2);

            // blow up if we aren't moving forward each step, which is
            // probably caused by interpenetration at the frame start

            assert(fabs(TargetTime - CurrentTime) > Epsilon);
        }
        else
        {
            // either colliding or clear

            if(CollisionState == Colliding)
            {
                // @todo handle multiple simultaneous collisions

                int Counter = 0;
                do
                {
                    ResolveCollisions(TargetConfigurationIndex);
                    Counter++;
                } while((CheckForCollisions(TargetConfigurationIndex) ==
                            Colliding) && (Counter < 100));

                assert(Counter < 100);
            }

            // we made a successful step, so swap configurations
            // to "save" the data for the next step

            CurrentTime = TargetTime;
            TargetTime = DeltaTime;

            SourceConfigurationIndex = SourceConfigurationIndex ? 0 : 1;
            TargetConfigurationIndex = TargetConfigurationIndex ? 0 : 1;
        }
    }
}

/*----------------------------------------------------------------------------

ComputeForces - compute forces for gravity, spring, etc.

*/

void simulation_world::ComputeForces( int ConfigurationIndex )
{
    int Counter;

    for(Counter = 0;Counter < NumberOfBodies;Counter++)
    {
        rigid_body &Body = aBodies[Counter];
        rigid_body::configuration &Configuration =
            Body.aConfigurations[ConfigurationIndex];

        // clear forces

        Configuration.Torque = r(0);
        Configuration.CMForce = vector_2(r(0),r(0));

        if(GravityActive)
        {
            Configuration.CMForce += Gravity / Body.OneOverMass;
        }

        if(DampingActive)
        {
            Configuration.CMForce += -Kdl * Configuration.CMVelocity;
            Configuration.Torque += -Kda * Configuration.AngularVelocity;
        }
    }

    if(BodySpringActive)
    {
        rigid_body &Body0 = aBodies[0];
        rigid_body::configuration &Configuration0 =
                Body0.aConfigurations[ConfigurationIndex];
        rigid_body::configuration::bounding_box &Box0 =
                Configuration0.BoundingBox;
        vector_2 Position0 = Box0.aVertices[Body0SpringVertexIndex];
        vector_2 U0 = Position0 - Configuration0.CMPosition;
        vector_2 VU0 = Configuration0.CMVelocity +
                        Configuration0.AngularVelocity * GetPerpendicular(U0);

        rigid_body &Body1 = aBodies[1];
        rigid_body::configuration &Configuration1 =
                Body1.aConfigurations[ConfigurationIndex];
        rigid_body::configuration::bounding_box &Box1 =
                Configuration1.BoundingBox;
        vector_2 Position1 = Box1.aVertices[Body1SpringVertexIndex];
        vector_2 U1 = Position1 - Configuration1.CMPosition;
        vector_2 VU1 = Configuration1.CMVelocity +
                        Configuration1.AngularVelocity * GetPerpendicular(U1);

        // spring goes from 0 to 1

        vector_2 SpringVector = Position1 - Position0;
        vector_2 Spring = -Kbs * SpringVector;

        vector_2 RelativeVelocity = VU1 - VU0;
        // project velocity onto spring to get damping vector
        // this is basically a Gram-Schmidt projection
        vector_2 DampingForce =
            -Kbd * (DotProduct(RelativeVelocity,SpringVector)/
                    DotProduct(SpringVector,SpringVector)) * SpringVector;

        Spring += DampingForce;

        Configuration0.CMForce -= Spring;
        Configuration0.Torque -= PerpDotProduct(U0,Spring);

        Configuration1.CMForce += Spring;
        Configuration1.Torque += PerpDotProduct(U1,Spring);
    }

    if(WorldSpringActive)
    {
        // apply spring to body 0's vertex 0 to anchor

        rigid_body &Body = aBodies[0];
        rigid_body::configuration &Configuration =
                Body.aConfigurations[ConfigurationIndex];
        rigid_body::configuration::bounding_box &Box =
                Configuration.BoundingBox;

        vector_2 Position = Box.aVertices[0];

        vector_2 U = Position - Configuration.CMPosition;
        vector_2 VU = Configuration.CMVelocity +
                        Configuration.AngularVelocity * GetPerpendicular(U);

        vector_2 Spring = -Kws * (Position - WorldSpringAnchor);
        // project velocity onto spring to get damping vector
        // this is basically a Gram-Schmidt projection
        vector_2 DampingForce =
            -Kwd * (DotProduct(VU,Spring)/DotProduct(Spring,Spring)) * Spring;

        Spring += DampingForce;

        Configuration.CMForce += Spring;
        Configuration.Torque += PerpDotProduct(U,Spring);
    }
}


/*----------------------------------------------------------------------------

CalculateVertices - figure out the body vertices from the configuration

@todo should't this be in the body?

*/

void simulation_world::CalculateVertices( int ConfigurationIndex )
{
    int Counter;

    for(Counter = 0;Counter < NumberOfBodies;Counter++)
    {
        matrix_2x2 const Rotation(
            aBodies[Counter].aConfigurations[ConfigurationIndex].
                Orientation);

        vector_2 const Position =
            aBodies[Counter].aConfigurations[ConfigurationIndex].
                CMPosition;

        rigid_body::configuration::bounding_box &Box =
            aBodies[Counter].aConfigurations[ConfigurationIndex].BoundingBox;

        real const Width = aBodies[Counter].Width / 2.0f;
        real const Height = aBodies[Counter].Height / 2.0f;

        Box.aVertices[0] = Position + Rotation * vector_2( Width, Height);
        Box.aVertices[1] = Position + Rotation * vector_2( Width,-Height);
        Box.aVertices[2] = Position + Rotation * vector_2(-Width,-Height);
        Box.aVertices[3] = Position + Rotation * vector_2(-Width, Height);
    }
}

/*----------------------------------------------------------------------------

Integrate - integrate the rigid body configurations forward in time from
                source config to target config

@todo calculate initial derivatives _once_
@todo use something better than Euler's method

*/

void simulation_world::Integrate( real DeltaTime )
{
    int Counter;

    for(Counter = 0;Counter < NumberOfBodies;Counter++)
    {
        rigid_body::configuration &Source =
            aBodies[Counter].aConfigurations[SourceConfigurationIndex];
        rigid_body::configuration &Target =
            aBodies[Counter].aConfigurations[TargetConfigurationIndex];

        Target.CMPosition = Source.CMPosition +
                DeltaTime * Source.CMVelocity;

        Target.Orientation = Source.Orientation +
                DeltaTime * Source.AngularVelocity;

        Target.CMVelocity = Source.CMVelocity +
                (DeltaTime * aBodies[Counter].OneOverMass) * Source.CMForce;

        Target.AngularVelocity = Source.AngularVelocity +
                (DeltaTime * aBodies[Counter].OneOverCMMomentOfInertia) *
                    Source.Torque;
    }
}


/*----------------------------------------------------------------------------

CheckForCollisions - test the current configuration for collisions

@todo handle multiple simultaneous collisions

*/

simulation_world::collision_state
simulation_world::CheckForCollisions( int ConfigurationIndex )
{
    // be optimistic!
    CollisionState = Clear;
    float const DepthEpsilon = 1.0f;

    real const HalfWidth = WorldWidth / 2.0f;
    real const HalfHeight = WorldHeight / 2.0f;

    for(int Body = 0;(Body < NumberOfBodies) &&
        (CollisionState != Penetrating);Body++)
    {
        // @todo active configuration number?!?!?
        rigid_body::configuration &Configuration =
                aBodies[Body].aConfigurations[ConfigurationIndex];

        rigid_body::configuration::bounding_box &Box =
                Configuration.BoundingBox;

        for(int Counter = 0;(Counter < 4) &&
            (CollisionState != Penetrating);Counter++)
        {
            vector_2 Position = Box.aVertices[Counter];

            vector_2 CMToCornerPerp =
                GetPerpendicular(Position - Configuration.CMPosition);

            vector_2 Velocity = Configuration.CMVelocity +
                Configuration.AngularVelocity * CMToCornerPerp;

            for(int WallIndex = 0;(WallIndex < NumberOfWalls) &&
                (CollisionState != Penetrating);WallIndex++)
            {
                wall &Wall = aWalls[WallIndex];

                real axbyc = DotProduct(Position,Wall.Normal) + Wall.c;

                if(axbyc < -DepthEpsilon)
                {
                    CollisionState = Penetrating;
                }
                else
                if(axbyc < DepthEpsilon)
                {
                    real RelativeVelocity = DotProduct(Wall.Normal,Velocity);

                    if(RelativeVelocity < r(0))
                    {
                        CollisionState = Colliding;
                        CollisionNormal = Wall.Normal;
                        CollidingCornerIndex = Counter;
                        CollidingBodyIndex = Body;
                    }
                }
            }
        }
    }

    return CollisionState;
}


/*----------------------------------------------------------------------------

ResolveCollisions - calculate collision impulses

@todo handle multiple simultaneous collisions

*/

void simulation_world::ResolveCollisions( int ConfigurationIndex )
{
    rigid_body &Body = aBodies[CollidingBodyIndex];
    rigid_body::configuration &Configuration =
                    Body.aConfigurations[ConfigurationIndex];

    vector_2 Position =
                    Configuration.BoundingBox.aVertices[CollidingCornerIndex];

    vector_2 CMToCornerPerp = GetPerpendicular(Position -
                    Configuration.CMPosition);

    vector_2 Velocity = Configuration.CMVelocity +
                    Configuration.AngularVelocity * CMToCornerPerp;

    real ImpulseNumerator = -(r(1) + Body.CoefficientOfRestitution) *
                    DotProduct(Velocity,CollisionNormal);

    float PerpDot = DotProduct(CMToCornerPerp,CollisionNormal);

    real ImpulseDenominator = Body.OneOverMass +
            Body.OneOverCMMomentOfInertia * PerpDot * PerpDot;

    real Impulse = ImpulseNumerator / ImpulseDenominator;

    Configuration.CMVelocity += Impulse * Body.OneOverMass * CollisionNormal;

    Configuration.AngularVelocity +=
                            Impulse * Body.OneOverCMMomentOfInertia * PerpDot;
}

/*----------------------------------------------------------------------------

2D Physics Test Program - a cheesy test harness for 2D physics

by Chris Hecker for my Game Developer Magazine articles.  See my homepage
for more information.

NOTE: This is a hacked test program, not a nice example of Windows programming.
physics.cpp the only part of this you should look at!!!

This material is Copyright 1997 Chris Hecker, All Rights Reserved.
It's for you to read and learn from, not to put in your own articles
or books or on your website, etc.  Thank you.

Chris Hecker
checker@d6.com
http://www.d6.com/users/checker

*/

/*----------------------------------------------------------------------------

physics.h - The header for the physics demo.

10/15/96 - checker

*/

#if !defined(PHYSICS_H)
#define PHYSICS_H

// explicit dependencies
#include "math2d.h"



/*----------------------------------------------------------------------------

Declarations for physics code.

*/

struct rigid_body
{
    real Width, Height;
    real OneOverMass, OneOverCMMomentOfInertia;
    real CoefficientOfRestitution;

    enum { NumberOfConfigurations = 2 };

    struct configuration
    {
        vector_2 CMPosition;
        real Orientation;

        vector_2 CMVelocity;
        real AngularVelocity;

        vector_2 CMForce;
        real Torque;

        struct bounding_box
        {
            vector_2 aVertices[4];

        } BoundingBox;

    } aConfigurations[NumberOfConfigurations];
};

class simulation_world
{
public:

    simulation_world( real WorldWidth, real WorldHeight );

    void Simulate( real DeltaTime );
    void Render( void );

    ~simulation_world( void );

private:

    enum collision_state
    {
        Penetrating,
        Colliding,
        Clear
    } CollisionState;

    vector_2 CollisionNormal;
    int CollidingBodyIndex;
    int CollidingCornerIndex;

    int SourceConfigurationIndex;
    int TargetConfigurationIndex;

    void ComputeForces( int ConfigurationIndex );
    void Integrate( real DeltaTime );
    collision_state CheckForCollisions( int ConfigurationIndex );
    void ResolveCollisions( int ConfigurationIndex );
    void CalculateVertices( int ConfigurationIndex );

    real WorldWidth, WorldHeight;

    enum { NumberOfWalls = 5 };
    struct wall
    {
        // define wall by plane equation
        vector_2 Normal;        // inward pointing
        real c;                 // ax + by + c = 0

        // points for drawing wall
        vector_2 StartPoint;
        vector_2 EndPoint;
    } aWalls[NumberOfWalls];

    enum { NumberOfBodies = 2 };
    rigid_body aBodies[NumberOfBodies];
};

#endif

/*----------------------------------------------------------------------------

2D Physics Test Program - a cheesy test harness for 2D physics

by Chris Hecker for my Game Developer Magazine articles.  See my homepage
for more information.

NOTE: This is a hacked test program, not a nice example of Windows programming.
physics.cpp the only part of this you should look at!!!

This material is Copyright 1997 Chris Hecker, All Rights Reserved.
It's for you to read and learn from, not to put in your own articles
or books or on your website, etc.  Thank you.

Chris Hecker
checker@d6.com
http://www.d6.com/users/checker

*/

#include "windows.h"
#include "win32.h"

AppIcon  ICON   physics.ico

AppAbout DIALOG DISCARDABLE  0, 0, 255, 86
STYLE DS_MODALFRAME | WS_POPUP | WS_VISIBLE | WS_CAPTION | WS_SYSMENU
CAPTION "About the Cheesy Physics App"
FONT 8, "MS Sans Serif"
BEGIN
    DEFPUSHBUTTON   "OK",IDOK,5,65,50,14
    LTEXT           "Cheesy 2D Physics Test App for my Game Developer Magazine articles",
                    -1,5,5,250,8
    LTEXT           "by Chris Hecker (checker@netcom.com)",-1,5,15,150,8
    LTEXT           "http://ourworld.compuserve.com/homepages/checker",-1,5,25,250,8
    ICON            "AppIcon",-1,225,30,0,0
END

AppMenu menu
begin
    POPUP "&File"
    BEGIN
        MENUITEM "&About",  MENU_ABOUT
    END
    POPUP "&Toggles"
    BEGIN
        MENUITEM "&WorldSpringaW", MENU_WORLDSPRING
        MENUITEM "&BodySpringaB",  MENU_BODYSPRING
        MENUITEM "&DampingaD", MENU_DAMPING
        MENUITEM "&GravityaG", MENU_GRAVITY
    END
end

/*  strstream.h -- class strstream declarations

*/

/*
 *      C/C++ Run Time Library - Version 6.5
 *
 *      Copyright (c) 1990, 1994 by Borland International
 *      All Rights Reserved.
 *
 */

#ifndef __cplusplus
#error Must use C++ for the type strstream.
#endif

#ifndef __STRSTREAM_H
#define __STRSTREAM_H

#if !defined(___DEFS_H)
#include <_defs.h>
#endif

#if !defined(__IOSTREAM_H)
#include <iostream.h>
#endif


#if !defined(RC_INVOKED)

#pragma option -a-

#if defined(__BCOPT__)
#if !defined(_RTL_ALLOW_po) && !defined(__FLAT__)
#pragma option -po-     // disable Object data calling convention
#endif
#endif

#if !defined(__TINY__)
#pragma option -RT
#endif

#pragma option -Vo-

#if defined(__STDC__)
#pragma warn -nak
#endif

#endif  /* !RC_INVOKED */


_CLASSDEF(strstreambuf)
_CLASSDEF(strstreambase)
_CLASSDEF(istrstream)
_CLASSDEF(ostrstream)
_CLASSDEF(strstream)

class _EXPCLASS strstreambuf : public streambuf {
public:
    _RTLENTRY strstreambuf();
    _RTLENTRY strstreambuf(int n);
    _RTLENTRY strstreambuf(void _FAR * (*a)(long), void (*f)(void _FAR *));
    _RTLENTRY strstreambuf(         char _FAR * _s, int,
                                    char _FAR * _strt=0);
    _RTLENTRY strstreambuf(signed   char _FAR * _s, int,
                           signed   char _FAR * _strt=0);
    _RTLENTRY strstreambuf(unsigned char _FAR * _s, int,
                           unsigned char _FAR * _strt=0);
    _RTLENTRY ~strstreambuf();

    void    _RTLENTRY freeze(int = 1);
    char _FAR *  _RTLENTRY str();
virtual int _RTLENTRY doallocate();
virtual int _RTLENTRY overflow(int);
virtual int _RTLENTRY underflow();
virtual int _RTLENTRY sync();
virtual streambuf _FAR * _RTLENTRY setbuf(char _FAR *, int);
virtual streampos   _RTLENTRY seekoff(streamoff, ios::seek_dir, int);

private:
    void _FAR *  _RTLENTRY (*allocf)(long);
    void    _RTLENTRY (*freef)(void _FAR *);
    short   ssbflags;
    enum    { dynamic = 1, frozen = 2, unlimited = 4 };
    int next_alloc;

    void    _RTLENTRY init(char _FAR *, int, char _FAR *);
};


class _EXPCLASS strstreambase : public virtual ios {
public:
    strstreambuf _FAR * _RTLENTRY rdbuf();

protected:
        _RTLENTRY strstreambase(char _FAR *, int, char _FAR *);
        _RTLENTRY strstreambase();
        _RTLENTRY ~strstreambase();
private:
        strstreambuf buf;
};
inline strstreambuf _FAR * _RTLENTRY strstreambase::rdbuf()
                                    { return &this->buf; }


class _EXPCLASS istrstream : public strstreambase, public istream {
public:
        _RTLENTRY istrstream(         char _FAR *);
        _RTLENTRY istrstream(signed   char _FAR *);
        _RTLENTRY istrstream(unsigned char _FAR *);
        _RTLENTRY istrstream(         char _FAR *, int);
        _RTLENTRY istrstream(signed   char _FAR *, int);
        _RTLENTRY istrstream(unsigned char _FAR *, int);
        _RTLENTRY ~istrstream();
};


class _EXPCLASS ostrstream : public strstreambase, public ostream {
public:
        _RTLENTRY ostrstream(         char _FAR *, int, int = ios::out);
        _RTLENTRY ostrstream(signed   char _FAR *, int, int = ios::out);
        _RTLENTRY ostrstream(unsigned char _FAR *, int, int = ios::out);
        _RTLENTRY ostrstream();
        _RTLENTRY ~ostrstream();

    char _FAR * _RTLENTRY str();
    int     _RTLENTRY pcount();
};
inline char _FAR * _RTLENTRY ostrstream::str()
                { return strstreambase::rdbuf()->str(); }
inline int  _RTLENTRY ostrstream::pcount()
                { return strstreambase::rdbuf()->out_waiting(); }


class _EXPCLASS strstream : public strstreambase, public iostream {
public:
        _RTLENTRY strstream();
        _RTLENTRY strstream(         char _FAR *, int _sz, int _m);
        _RTLENTRY strstream(signed   char _FAR *, int _sz, int _m);
        _RTLENTRY strstream(unsigned char _FAR *, int _sz, int _m);
        _RTLENTRY ~strstream();

    char _FAR * _RTLENTRY str();
};
inline char _FAR * _RTLENTRY strstream::str()
                { return strstreambase::rdbuf()->str(); }


#if !defined(RC_INVOKED)

#if defined(__STDC__)
#pragma warn .nak
#endif

#pragma option -Vo.

#if !defined(__TINY__)
#pragma option -RT.
#endif

#if defined(__BCOPT__)
#if !defined(_RTL_ALLOW_po) && !defined(__FLAT__)
#pragma option -po.     // restore Object data calling convention
#endif
#endif

#pragma option -a.  /* restore default packing */

#endif  /* !RC_INVOKED */


#endif  /* __STRSTREAM_H */

/*----------------------------------------------------------------------------

2D Physics Test Program - a cheesy test harness for 2D physics

by Chris Hecker for my Game Developer Magazine articles.  See my homepage
for more information.

NOTE: This is a hacked test program, not a nice example of Windows programming.
physics.cpp the only part of this you should look at!!!

This material is Copyright 1997 Chris Hecker, All Rights Reserved.
It's for you to read and learn from, not to put in your own articles
or books or on your website, etc.  Thank you.

Chris Hecker
checker@d6.com
http://www.d6.com/users/checker

*/

#define WIN32_LEAN_AND_MEAN
#define WIN32_EXTRA_LEAN
#include <windows.h>
#include <windowsx.h>
#include <mmsystem.h>
#include <commdlg.h>
#include <stdlib.h>
#include <math.h>
#include <assert.h>

#include "win32.h"
#include "iface.h"

#pragma warning (disable:4244) // conversion from float to int

/*----------------------------------------------------------------------------

Globals and declarations

*/

char szAppName[200] = "Cheesy 2D Physics App";

HINSTANCE hInstApp;
HWND      hwndApp;
HPALETTE  hpalApp;
BOOL fAppActive;

LONG FAR PASCAL AppWndProc(HWND hwnd,UINT msg,WPARAM wParam,LPARAM lParam);
LONG AppCommand (HWND hwnd,UINT msg,WPARAM wParam,LPARAM lParam);
int AppPaint (HWND hwnd, HDC hdc);

void AppExit(void);
BOOL AppIdle(void);

HBITMAP OldBitmap;
HDC BufferDC;
HBITMAP BufferBitmap;

/*----------------------------------------------------------------------------*
|   AppAbout( hDlg, uiMessage, wParam, lParam )                                |
|                                                                              |
|   Description:                                                               |
|       This function handles messages belonging to the "About" dialog box.    |
|       The only message that it looks for is WM_COMMAND, indicating the use   |
|       has pressed the "OK" button.  When this happens, it takes down         |
|       the dialog box.                                                        |
|                                                                              |
|   Arguments:                                                                 |
|       hDlg            window handle of about dialog window                   |
|       uiMessage       message number                                         |
|       wParam          message-dependent                                      |
|       lParam          message-dependent                                      |
|                                                                              |
|   Returns:                                                                   |
|       TRUE if message has been processed, else FALSE                         |
|                                                                              |
*----------------------------------------------------------------------------*/
BOOL FAR PASCAL  AppAbout(HWND hwnd,UINT msg,WPARAM wParam,
            LPARAM /* lParam */ )
{
    switch (msg)
    {
        case WM_COMMAND:
            if (LOWORD(wParam) == IDOK)
            {
                EndDialog(hwnd,TRUE);
            }
            break;

        case WM_INITDIALOG:
            return TRUE;
    }
    return FALSE;
}

/*----------------------------------------------------------------------------*
|   AppInit( hInst, hPrev)                                                     |
|                                                                              |
|   Description:                                                               |
|       This is called when the application is first loaded into               |
|       memory.  It performs all initialization that doesn't need to be done   |
|       once per instance.                                                     |
|                                                                              |
|   Arguments:                                                                 |
|       hInstance       instance handle of current instance                    |
|       hPrev           instance handle of previous instance                   |
|                                                                              |
|   Returns:                                                                   |
|       TRUE if successful, FALSE if not                                       |
|                                                                              |
*----------------------------------------------------------------------------*/
BOOL AppInit(HINSTANCE hInst,HINSTANCE hPrev,int sw,LPSTR szCmdLine)
{
    WNDCLASS cls;

    /* Save instance handle for DialogBoxs */
    hInstApp = hInst;

    if (!hPrev)
    {
        /*
         *  Register a class for the main application window
         */
        cls.hCursor        = LoadCursor(NULL,IDC_ARROW);
        cls.hIcon          = LoadIcon(hInst,"AppIcon");
        cls.lpszMenuName   = "AppMenu";
        cls.lpszClassName  = szAppName;
        cls.hbrBackground  = (HBRUSH)(COLOR_WINDOW + 1);
        cls.hInstance      = hInst;
        cls.style          = CS_BYTEALIGNCLIENT | CS_VREDRAW | CS_HREDRAW | CS_DBLCLKS;
        cls.lpfnWndProc    = (WNDPROC)AppWndProc;
        cls.cbWndExtra     = 0;
        cls.cbClsExtra     = 0;

        if (!RegisterClass(&cls))
            return FALSE;
    }

    DWORD Style = WS_OVERLAPPEDWINDOW;
    RECT WindowRect = { 0, 0 };
    WindowRect.right = WorldWidth;
    WindowRect.bottom = WorldHeight;

    AdjustWindowRect(&WindowRect,Style,TRUE);

    hwndApp = CreateWindow(szAppName,szAppName,Style,
                    CW_USEDEFAULT,0,
                    WindowRect.right-WindowRect.left,
                    WindowRect.bottom-WindowRect.top,
                    0,0,hInst,0);



    // create buffer bitmap

    HDC Screen = GetDC(0);
    BufferDC = CreateCompatibleDC(0);
    BufferBitmap = CreateCompatibleBitmap(Screen,WorldWidth,WorldHeight);
    OldBitmap = SelectBitmap(BufferDC,BufferBitmap);
    ReleaseDC(0,Screen);

    ShowWindow(hwndApp,sw);

    return TRUE;
}


/*----------------------------------------------------------------------------*
|   AppExit()                                      |
|                                                                              |
|   Description:                                                               |
|   app is just about to exit, cleanup                     |
|                                                                              |
*----------------------------------------------------------------------------*/
void AppExit()
{
    SelectObject(BufferDC,OldBitmap);
    DeleteObject(BufferBitmap);
    DeleteDC(BufferDC);
}

BOOL AppIdle()
{
    if (fAppActive)
    {
        HDC WindowDC = GetDC(hwndApp);
        AppPaint(hwndApp,WindowDC);
        ReleaseDC(hwndApp,WindowDC);

        return FALSE;
    }
    else
    {
    //
    // we are a background app.
    //
    return TRUE;        // nothing to do.
    }
}

/*----------------------------------------------------------------------------*
|   WinMain( hInst, hPrev, lpszCmdLine, cmdShow )                              |
|                                                                              |
|   Description:                                                               |
|       The main procedure for the App.  After initializing, it just goes      |
|       into a message-processing loop until it gets a WM_QUIT message         |
|       (meaning the app was closed).                                          |
|                                                                              |
|   Arguments:                                                                 |
|       hInst           instance handle of this instance of the app            |
|       hPrev           instance handle of previous instance, NULL if first    |
|       szCmdLine       ->null-terminated command line                         |
|       cmdShow         specifies how the window is initially displayed        |
|                                                                              |
|   Returns:                                                                   |
|       The exit code as specified in the WM_QUIT message.                     |
|                                                                              |
*----------------------------------------------------------------------------*/
int PASCAL WinMain(HINSTANCE hInst, HINSTANCE hPrev, LPSTR szCmdLine, int sw)
{
    MSG     msg;

    /* Call initialization procedure */
    if (!AppInit(hInst,hPrev,sw,szCmdLine))
    return FALSE;

    /*
     * Polling messages from event queue
     */
    for (;;)
    {
        if (PeekMessage(&msg, NULL, 0, 0,PM_REMOVE))
        {
            if (msg.message == WM_QUIT)
                break;

            TranslateMessage(&msg);
            DispatchMessage(&msg);
        }
        else
        {
            if (AppIdle())
                WaitMessage();
        }
    }


    AppExit();
    return msg.wParam;
}

/*----------------------------------------------------------------------------*
|   AppPaint(hwnd, hdc)                                                        |
|                                                                              |
|   Description:                                                               |
|       The paint function.  Right now this does nothing.                      |
|                                                                              |
|   Arguments:                                                                 |
|       hwnd             window painting into                                  |
|       hdc              display context to paint to                           |
|                                                                              |
|   Returns:                                                                   |
|       nothing                                                                |
|                                                                              |
*----------------------------------------------------------------------------*/
int AppPaint (HWND hwnd, HDC hdc)
{
    RECT rc;
    GetClientRect(hwnd, &rc);

    PatBlt(BufferDC,0,0,WorldWidth,WorldHeight,WHITENESS);

    Run();      // run the physics

    BitBlt(hdc,0,0,rc.right,rc.bottom,BufferDC,0,0,SRCCOPY);

    return TRUE;
}

/*----------------------------------------------------------------------------*
|   AppWndProc( hwnd, uiMessage, wParam, lParam )                              |
|                                                                              |
|   Description:                                                               |
|       The window proc for the app's main (tiled) window.  This processes all |
|       of the parent window's messages.                                       |
|                                                                              |
*----------------------------------------------------------------------------*/
LONG FAR PASCAL AppWndProc(HWND hwnd,UINT msg,WPARAM wParam,LPARAM lParam)
{
    PAINTSTRUCT ps;
    HDC hdc;

    switch (msg)
    {
        case WM_CREATE:
        break;

        case WM_ACTIVATEAPP:
            fAppActive = (BOOL)wParam;
            break;

        case WM_KEYDOWN:
        {
            switch(wParam)
            {
                case 'W':
                {
                    SendMessage(hwnd,WM_COMMAND,MENU_WORLDSPRING,0);
                    break;
                }
                case 'B':
                {
                    SendMessage(hwnd,WM_COMMAND,MENU_BODYSPRING,0);
                    break;
                }
                case 'D':
                {
                    SendMessage(hwnd,WM_COMMAND,MENU_DAMPING,0);
                    break;
                }
                case 'G':
                {
                    SendMessage(hwnd,WM_COMMAND,MENU_GRAVITY,0);
                    break;
                }
                break;
            }
        }

        case WM_LBUTTONDOWN:
        {
            break;
        }

        case WM_MOUSEMOVE:
        {
            break;
        }

        case WM_LBUTTONUP:
        {
            break;
        }

        case WM_ERASEBKGND:
            break;

        case WM_INITMENU:
            break;

        case WM_COMMAND:
            return AppCommand(hwnd,msg,wParam,lParam);

        case WM_DESTROY:
            PostQuitMessage(0);
            break;

        case WM_CLOSE:
            break;

        case WM_PAINT:
            hdc = BeginPaint(hwnd,&ps);
            AppPaint (hwnd,hdc);
            EndPaint(hwnd,&ps);
            return 0L;

        case WM_SIZE:
            break;
    }
    return DefWindowProc(hwnd,msg,wParam,lParam);
}

/*----------------------------------------------------------------------------*
|   AppCommand(hwnd, msg, wParam, lParam )                     |
|                                                                              |
|   Description:                                                               |
|   handles WM_COMMAND messages for the main window (hwndApp)          |
|       of the parent window's messages.                                       |
|                                                                              |
*----------------------------------------------------------------------------*/
LONG AppCommand (HWND hwnd,UINT /* msg */,WPARAM wParam,LPARAM /* lParam */)
{
    switch(wParam)
    {
        case MENU_ABOUT:
            DialogBox(hInstApp,"AppAbout",hwnd,AppAbout);
            break;

        case MENU_WORLDSPRING:
            ToggleWorldSpring();
            break;
        case MENU_BODYSPRING:
            ToggleBodySpring();
            break;
        case MENU_DAMPING:
            ToggleDamping();
            break;
        case MENU_GRAVITY:
            ToggleGravity();
            break;

        case MENU_EXIT:
            PostMessage(hwnd,WM_CLOSE,0,0L);
            break;
    }
    return 0L;
}

extern "C" void __cdecl _assert( void *pExpression, void *pFile,
    unsigned LineNumber )
{
    char aBuffer[500];
    wsprintf(aBuffer,"Assertion: %snFile: %s, Line: %dn"
                    "Hit Abort to exit, Retry to debug, Ignore to continue",
                    pExpression,pFile,LineNumber);

    int Hit = MessageBox(hwndApp,aBuffer,"Assert!",MB_ABORTRETRYIGNORE |
                    MB_ICONHAND);

    if(Hit == IDABORT)
    {
        exit(0);
    }
    else
    if(Hit == IDRETRY)
    {
        DebugBreak();
    }
}

/*----------------------------------------------------------------------------

iface.h functions

*/

void Line( int X0, int Y0, int X1, int Y1 )
{
    MoveToEx(BufferDC,(WorldWidth/2)+X0,(WorldHeight/2)-Y0,0);
    LineTo(BufferDC,(WorldWidth/2)+X1,(WorldHeight/2)-Y1);
}

float GetTime( void )
{
    static DWORD StartMilliseconds;
    if(!StartMilliseconds)
    {
        // yes, the first time through will be a 0 timestep
        StartMilliseconds = timeGetTime();
    }

    DWORD CurrentMilliseconds = timeGetTime();
    return float(CurrentMilliseconds - StartMilliseconds) / 1000.0f;
}

/*----------------------------------------------------------------------------

2D Physics Test Program - a cheesy test harness for 2D physics

by Chris Hecker for my Game Developer Magazine articles.  See my homepage
for more information.

NOTE: This is a hacked test program, not a nice example of Windows programming.
physics.cpp the only part of this you should look at!!!

This material is Copyright 1997 Chris Hecker, All Rights Reserved.
It's for you to read and learn from, not to put in your own articles
or books or on your website, etc.  Thank you.

Chris Hecker
checker@d6.com
http://www.d6.com/users/checker

*/

/* Menu Items */
#define MENU_ABOUT          0
#define MENU_EXIT           1
#define MENU_WORLDSPRING    2
#define MENU_BODYSPRING     3
#define MENU_DAMPING        4
#define MENU_GRAVITY        5