////////////////////////////////////////////////////////////////////////////////
// Copyright (C) 2012  Daniele Giannetti
//
// This code is provided to evaluate some features of the Free binary-only
// Havok Physics & Animation distribution. By no means we guarantee this small
// project to be bug-free. This code implements a simple 3D physics-based game
// using Havok Physics for real time physical simulation, and HKG to provide
// some visual feedback to the user.
////////////////////////////////////////////////////////////////////////////////
#include <stdlib.h>
//#include <GL/freeglut.h>
#include <gl/glut.h>
//#include <game.h>
#include <string>



#include <Common/Base/hkBase.h>
#include <Common/Base/Memory/System/Util/hkMemoryInitUtil.h>
#include <Common/Base/Memory/Allocator/Malloc/hkMallocAllocator.h>
// Physics includes
#include <Physics/Collide/hkpCollide.h>
#include <Physics/Collide/Shape/Convex/Sphere/hkpSphereShape.h>
#include <Physics/Collide/Shape/Convex/Box/hkpBoxShape.h>
#include <Physics/Collide/Shape/Compound/Collection/List/hkpListShape.h>
#include <Physics/Collide/Shape/Convex/ConvexTranslate/hkpConvexTranslateShape.h>
#include <Physics/Collide/Shape/Misc/Bv/hkpBvShape.h>
#include <Physics/Collide/Shape/Misc/PhantomCallback/hkpPhantomCallbackShape.h>
#include <Physics/Collide/Dispatch/hkpAgentRegisterUtil.h>
#include <Physics/Dynamics/hkpDynamics.h>
#include <Physics/Dynamics/World/hkpWorld.h>
#include <Physics/Dynamics/Entity/hkpRigidBody.h>
#include <Physics/Dynamics/Constraint/ConstraintKit/hkpGenericConstraintData.h>
#include <Physics/Dynamics/Constraint/ConstraintKit/hkpConstraintConstructionKit.h>
#include <Physics/Dynamics/Constraint/hkpConstraintInstance.h>
#include <Physics/Utilities/Dynamics/Inertia/hkpInertiaTensorComputer.h>
#ifdef DEBUG
	// Visual Debugger includes
	#include <Common/Visualize/hkVisualDebugger.h>
	#include <Physics/Utilities/VisualDebugger/hkpPhysicsContext.h>
#endif


#include <iostream>

	hkpWorld* m_world;





#include <Physics/Vehicle/hkpVehicleInstance.h>
#include <Physics/Vehicle/Camera/hkp1dAngularFollowCam.h>


















#include <Physics/Vehicle/DriverInput/Default/hkpVehicleDefaultAnalogDriverInput.h>















#include <Physics/Vehicle/hkpVehicleInstance.h>

#include <Physics/Vehicle/AeroDynamics/Default/hkpVehicleDefaultAerodynamics.h>
#include <Physics/Vehicle/DriverInput/Default/hkpVehicleDefaultAnalogDriverInput.h>
#include <Physics/Vehicle/Brake/Default/hkpVehicleDefaultBrake.h>
#include <Physics/Vehicle/Engine/Default/hkpVehicleDefaultEngine.h>
#include <Physics/Vehicle/VelocityDamper/Default/hkpVehicleDefaultVelocityDamper.h>
#include <Physics/Vehicle/Steering/Default/hkpVehicleDefaultSteering.h>
#include <Physics/Vehicle/Suspension/Default/hkpVehicleDefaultSuspension.h>
#include <Physics/Vehicle/Transmission/Default/hkpVehicleDefaultTransmission.h>
#include <Physics/Vehicle/WheelCollide/RayCast/hkpVehicleRayCastWheelCollide.h>
#include <Physics/Vehicle/TyreMarks/hkpTyremarksInfo.h>


	/// This class just holds ALL the vehicle parameters (hardcoded) for an untuned, four
	

	


#include <Physics/Vehicle/hkpVehicleInstance.h>

#include <Physics/Vehicle/AeroDynamics/Default/hkpVehicleDefaultAerodynamics.h>
#include <Physics/Vehicle/DriverInput/Default/hkpVehicleDefaultAnalogDriverInput.h>
#include <Physics/Vehicle/Brake/Default/hkpVehicleDefaultBrake.h>
#include <Physics/Vehicle/Engine/Default/hkpVehicleDefaultEngine.h>
#include <Physics/Vehicle/VelocityDamper/Default/hkpVehicleDefaultVelocityDamper.h>
#include <Physics/Vehicle/Steering/Default/hkpVehicleDefaultSteering.h>
#include <Physics/Vehicle/Suspension/Default/hkpVehicleDefaultSuspension.h>
#include <Physics/Vehicle/Transmission/Default/hkpVehicleDefaultTransmission.h>
#include <Physics/Vehicle/WheelCollide/RayCast/hkpVehicleRayCastWheelCollide.h>
#include <Physics/Vehicle/TyreMarks/hkpTyremarksInfo.h>

	/// This class just holds ALL the vehicle parameters (hardcoded) for an untuned, four
	/// wheeled vehicle and facilitates construction via blueprints on a single call to
	/// buildVehicle().












#include <Common\Base\Object\hkBaseObject.h>
#include <Common\Base\Object\hkReferencedObject.h>
#include <Common\Base\Types\Geometry\hkStridedVertices.h>
#include <Physics\Collide\Shape\Convex\ConvexVertices\hkpConvexVerticesShape.h>





#ifndef FRICTION_MAP_VEHICLE_RAYCAST_COLLIDE_H
#define FRICTION_MAP_VEHICLE_RAYCAST_COLLIDE_H

#include <Physics/Vehicle/WheelCollide/RayCast/hkpVehicleRayCastWheelCollide.h>
#include <Physics/Vehicle/hkpVehicleInstance.h>
#include <Physics/Collide/Query/CastUtil/hkpWorldRayCastOutput.h>

extern const class hkClass FrictionMapVehicleRaycastWheelCollideClass;

#define VEHICLE_RAYCAST_ICY_FRICTION_PROPERTY 67341

	// This is the class which implements the calcSingleWheelGroundFriction, allowing you to override the
	// ground friction calculation for raycast vehicles. Here, as a very simple example, we overwrite the friction to be
	// 0.01 for all bodies (upon which we are driving) which have been tagged with a VEHICLE_RAYCAST_ICY_FRICTION_PROPERTY
	// user property, otherwise we leave the friction as it is.
	// In general of course, the friction value need not be constant over the body (or time), and any function can be used 
	// here, for example a lookup into your own full 2D 'friction map'.
	// Other ways to store/retrieve friction values would be via:
	//	hkpShape user data
	//	A 'float' hkpProperty stored with a the rigid body
	//	hkMeshMaterials for hkMeshShapes
	//
	
class FrictionMapVehicleRaycastWheelCollide : public hkpVehicleRayCastWheelCollide
{
	public:
		HK_DECLARE_CLASS_ALLOCATOR(HK_MEMORY_CLASS_BASE);

	private:
		virtual void wheelCollideCallback( const hkpVehicleInstance* vehicle, hkUint8 wheelIndex, CollisionDetectionWheelOutput& cdInfo )
		{
			hkpRigidBody* rb = cdInfo.m_contactBody;
			if ( rb )
			{
				if ( rb->hasProperty(VEHICLE_RAYCAST_ICY_FRICTION_PROPERTY) )
				{
					cdInfo.m_contactFriction = 0.01f;
				} 
			}
		}

	// Serialization.

	public:
		// By adding HK_DECLARE_REFLECTION, we enable objects of this class to be serialized.
		// However, the class does need to be registered with the type registry. The following code can be used:
		// hkBuiltinTypeRegistry::getInstance().addType( &FrictionMapVehicleRaycastWheelCollideTypeInfo, &FrictionMapVehicleRaycastWheelCollideClass );
		HK_DECLARE_REFLECTION();

		FrictionMapVehicleRaycastWheelCollide () { }

		FrictionMapVehicleRaycastWheelCollide ( hkFinishLoadedObjectFlag f ) : hkpVehicleRayCastWheelCollide( f ) { }
};

#endif // FRICTION_MAP_VEHICLE_RAYCAST_COLLIDE_H



#ifndef HK_VEHICLE_SETUP_H
#define HK_VEHICLE_SETUP_H

#include <Physics/Vehicle/hkpVehicleInstance.h>

#include <Physics/Vehicle/AeroDynamics/Default/hkpVehicleDefaultAerodynamics.h>
#include <Physics/Vehicle/DriverInput/Default/hkpVehicleDefaultAnalogDriverInput.h>
#include <Physics/Vehicle/Brake/Default/hkpVehicleDefaultBrake.h>
#include <Physics/Vehicle/Engine/Default/hkpVehicleDefaultEngine.h>
#include <Physics/Vehicle/VelocityDamper/Default/hkpVehicleDefaultVelocityDamper.h>
#include <Physics/Vehicle/Steering/Default/hkpVehicleDefaultSteering.h>
#include <Physics/Vehicle/Suspension/Default/hkpVehicleDefaultSuspension.h>
#include <Physics/Vehicle/Transmission/Default/hkpVehicleDefaultTransmission.h>
#include <Physics/Vehicle/WheelCollide/RayCast/hkpVehicleRayCastWheelCollide.h>
#include <Physics/Vehicle/TyreMarks/hkpTyremarksInfo.h>

	/// This class just holds ALL the vehicle parameters (hardcoded) for an untuned, four
	/// wheeled vehicle and facilitates construction via blueprints on a single call to
	/// buildVehicle().
class VehicleSetup : public hkReferencedObject
{
	public:

		HK_DECLARE_CLASS_ALLOCATOR( HK_MEMORY_CLASS_VEHICLE );

		VehicleSetup(){}

		virtual void buildVehicle( const hkpWorld* world, hkpVehicleInstance& vehicle ); 

	public:

		virtual void setupVehicleData( const hkpWorld* world, hkpVehicleData& data );
		virtual void setupComponent( const hkpVehicleData& data, hkpVehicleDefaultAnalogDriverInput& driverInput );
		virtual void setupComponent( const hkpVehicleData& data, hkpVehicleDefaultSteering& steering );
		virtual void setupComponent( const hkpVehicleData& data, hkpVehicleDefaultEngine& engine );
		virtual void setupComponent( const hkpVehicleData& data, hkpVehicleDefaultTransmission& transmission );
		virtual void setupComponent( const hkpVehicleData& data, hkpVehicleDefaultBrake& brake );
		virtual void setupComponent( const hkpVehicleData& data, hkpVehicleDefaultSuspension& suspension );
		virtual void setupComponent( const hkpVehicleData& data, hkpVehicleDefaultAerodynamics& aerodynamics );
		virtual void setupComponent( const hkpVehicleData& data, hkpVehicleDefaultVelocityDamper& velocityDamper );

		virtual void setupWheelCollide( const hkpWorld* world, const hkpVehicleInstance& vehicle, hkpVehicleRayCastWheelCollide& wheelCollide );
		virtual void setupTyremarks( const hkpVehicleData& data, hkpTyremarksInfo& tyremarkscontroller );
};

#endif // HK_VEHICLE_SETUP_H





void VehicleSetup::buildVehicle( const hkpWorld* world, hkpVehicleInstance& vehicle )
{
	//
	// All memory allocations are made here.
	//

	vehicle.m_data				= new hkpVehicleData;
	vehicle.m_driverInput		= new hkpVehicleDefaultAnalogDriverInput;
	vehicle.m_steering			= new hkpVehicleDefaultSteering;
	vehicle.m_engine			= new hkpVehicleDefaultEngine;
	vehicle.m_transmission		= new hkpVehicleDefaultTransmission;
	vehicle.m_brake				= new hkpVehicleDefaultBrake;
	vehicle.m_suspension		= new hkpVehicleDefaultSuspension;
	vehicle.m_aerodynamics		= new hkpVehicleDefaultAerodynamics;
	vehicle.m_velocityDamper	= new hkpVehicleDefaultVelocityDamper;

	// For illustrative purposes we use a custom hkpVehicleRayCastWheelCollide
	// which implements varying 'ground' friction in a very simple way.
	vehicle.m_wheelCollide		= new FrictionMapVehicleRaycastWheelCollide;

	setupVehicleData( world, *vehicle.m_data );

	// initialise the tyremarks controller with 128 tyremark points.
	vehicle.m_tyreMarks		= new hkpTyremarksInfo( *vehicle.m_data, 128 );

	setupComponent( *vehicle.m_data, *static_cast< hkpVehicleDefaultAnalogDriverInput* >(vehicle.m_driverInput) );
	setupComponent( *vehicle.m_data, *static_cast< hkpVehicleDefaultSteering*>(vehicle.m_steering));
	setupComponent( *vehicle.m_data, *static_cast< hkpVehicleDefaultEngine*>(vehicle.m_engine) );
	setupComponent( *vehicle.m_data, *static_cast< hkpVehicleDefaultTransmission*>(vehicle.m_transmission) );
	setupComponent( *vehicle.m_data, *static_cast< hkpVehicleDefaultBrake*>(vehicle.m_brake) );
	setupComponent( *vehicle.m_data, *static_cast< hkpVehicleDefaultSuspension*>(vehicle.m_suspension) );
	setupComponent( *vehicle.m_data, *static_cast< hkpVehicleDefaultAerodynamics*>(vehicle.m_aerodynamics) );
	setupComponent( *vehicle.m_data, *static_cast< hkpVehicleDefaultVelocityDamper*>(vehicle.m_velocityDamper) );

	setupWheelCollide( world, vehicle, *static_cast< hkpVehicleRayCastWheelCollide*>(vehicle.m_wheelCollide) );

	setupTyremarks( *vehicle.m_data, *static_cast< hkpTyremarksInfo*>(vehicle.m_tyreMarks) );

	//
	// Check that all components are present.
	//
	HK_ASSERT(0x0, vehicle.m_data );
	HK_ASSERT(0x7708674a,  vehicle.m_driverInput );
	HK_ASSERT(0x5a324a2d,  vehicle.m_steering );
	HK_ASSERT(0x7bcb2aff,  vehicle.m_engine );
	HK_ASSERT(0x29bddb50,  vehicle.m_transmission );
	HK_ASSERT(0x2b0323a2,  vehicle.m_brake );
	HK_ASSERT(0x7a7ade23,  vehicle.m_suspension );
	HK_ASSERT(0x6ec4d0ed,  vehicle.m_aerodynamics );
	HK_ASSERT(0x67161206,  vehicle.m_wheelCollide );
	HK_ASSERT(0x295015f1,  vehicle.m_tyreMarks );

	//
	// Set up any variables that store cached data.
	//


	// Give driver input default values so that the vehicle (if this input is a default for non
	// player cars) will drive, even if it is in circles!

	// Accelerate.
	vehicle.m_deviceStatus = new hkpVehicleDriverInputAnalogStatus;
	hkpVehicleDriverInputAnalogStatus* deviceStatus = (hkpVehicleDriverInputAnalogStatus*)vehicle.m_deviceStatus;
	deviceStatus->m_positionY = -0.4f;

	// Turn.
	deviceStatus->m_positionX = 0.3f;

	// Defaults
	deviceStatus->m_handbrakeButtonPressed = false;
	deviceStatus->m_reverseButtonPressed = false;


	//
	// Don't forget to call init! (This function is necessary to set up derived data)
	//
	vehicle.init();
}

void VehicleSetup::setupVehicleData( const hkpWorld* world, hkpVehicleData& data )
{
	data.m_gravity = world->getGravity();

	//
	// The vehicleData contains information about the chassis.
	//

	// The coordinates of the chassis system, used for steering the vehicle.
	//										up					forward				right
	data.m_chassisOrientation.setCols( hkVector4(0, 1, 0), hkVector4(1, 0, 0), hkVector4(0, 0, 1));

	data.m_frictionEqualizer = 0.5f; 


	// Inertia tensor for each axis is calculated by using : 
	// (1 / chassis_mass) * (torque(axis)Factor / chassisUnitInertia)
	data.m_torqueRollFactor = 0.625f; 
	data.m_torquePitchFactor = 0.5f; 
	data.m_torqueYawFactor = 0.35f; 

	data.m_chassisUnitInertiaYaw = 1.0f; 
	data.m_chassisUnitInertiaRoll = 1.0f;
	data.m_chassisUnitInertiaPitch = 1.0f; 

	// Adds or removes torque around the yaw axis 
	// based on the current steering angle.  This will 
	// affect steering.
	data.m_extraTorqueFactor = -0.5f; 
	data.m_maxVelocityForPositionalFriction = 0.0f; 

	//
	// Wheel specifications
	//
	data.m_numWheels = 4;

	data.m_wheelParams.setSize( data.m_numWheels );

	data.m_wheelParams[0].m_axle = 0;
	data.m_wheelParams[1].m_axle = 0;
	data.m_wheelParams[2].m_axle = 1;
	data.m_wheelParams[3].m_axle = 1;

	data.m_wheelParams[0].m_friction = 1.5f;
	data.m_wheelParams[1].m_friction = 1.5f;
	data.m_wheelParams[2].m_friction = 1.5f;
	data.m_wheelParams[3].m_friction = 1.5f;

	data.m_wheelParams[0].m_slipAngle = 0.0f;
	data.m_wheelParams[1].m_slipAngle = 0.0f;
	data.m_wheelParams[2].m_slipAngle = 0.0f;
	data.m_wheelParams[3].m_slipAngle = 0.0f;

	for ( int i = 0 ; i < data.m_numWheels ; i++ )
	{
		// This value is also used to calculate the m_primaryTransmissionRatio.
		data.m_wheelParams[i].m_radius = 0.4f;
		data.m_wheelParams[i].m_width = 0.2f;
		data.m_wheelParams[i].m_mass = 10.0f;

		data.m_wheelParams[i].m_viscosityFriction = 0.25f;
		data.m_wheelParams[i].m_maxFriction = 2.0f * data.m_wheelParams[i].m_friction;
		data.m_wheelParams[i].m_forceFeedbackMultiplier = 0.1f;
		data.m_wheelParams[i].m_maxContactBodyAcceleration = hkReal(data.m_gravity.length3()) * 2;
	}
}


void VehicleSetup::setupComponent( const hkpVehicleData& data, hkpVehicleDefaultAnalogDriverInput& driverInput )
{
	// We also use an analog "driver input" class to help converting user input to vehicle behavior.

	driverInput.m_slopeChangePointX = 0.8f;
	driverInput.m_initialSlope = 0.7f;
	driverInput.m_deadZone = 0.0f;
	driverInput.m_autoReverse = true;
}

void VehicleSetup::setupComponent( const hkpVehicleData& data, hkpVehicleDefaultSteering& steering )
{   
	steering.m_doesWheelSteer.setSize( data.m_numWheels );

	// degrees
	steering.m_maxSteeringAngle = 35 * ( HK_REAL_PI / 180 );

	// [mph/h] The steering angle decreases linearly 
	// based on your overall max speed of the vehicle. 
	steering.m_maxSpeedFullSteeringAngle = 70.0f * (1.605f / 3.6f);
	steering.m_doesWheelSteer[0] = true;
	steering.m_doesWheelSteer[1] = true;
	steering.m_doesWheelSteer[2] = false;
	steering.m_doesWheelSteer[3] = false;
}

void VehicleSetup::setupComponent( const hkpVehicleData& data, hkpVehicleDefaultEngine& engine )
{
	engine.m_maxTorque = 500.0f;

	engine.m_minRPM = 1000.0f;
	engine.m_optRPM = 5500.0f;

	// This value is also used to calculate the m_primaryTransmissionRatio.
	engine.m_maxRPM = 7500.0f;

	engine.m_torqueFactorAtMinRPM = 0.8f;
	engine.m_torqueFactorAtMaxRPM = 0.8f;

	engine.m_resistanceFactorAtMinRPM = 0.05f;
	engine.m_resistanceFactorAtOptRPM = 0.1f;
	engine.m_resistanceFactorAtMaxRPM = 0.3f;
}

void VehicleSetup::setupComponent( const hkpVehicleData& data, hkpVehicleDefaultTransmission& transmission )
{
	int numGears = 4;

	transmission.m_gearsRatio.setSize( numGears );
	transmission.m_wheelsTorqueRatio.setSize( data.m_numWheels );

	transmission.m_downshiftRPM = 3500.0f;
	transmission.m_upshiftRPM = 6500.0f;

	transmission.m_clutchDelayTime = 0.0f;
	transmission.m_reverseGearRatio = 1.0f;
	transmission.m_gearsRatio[0] = 2.0f;
	transmission.m_gearsRatio[1] = 1.5f;
	transmission.m_gearsRatio[2] = 1.0f;
	transmission.m_gearsRatio[3] = 0.75f;
	transmission.m_wheelsTorqueRatio[0] = 0.2f;
	transmission.m_wheelsTorqueRatio[1] = 0.2f;
	transmission.m_wheelsTorqueRatio[2] = 0.3f;
	transmission.m_wheelsTorqueRatio[3] = 0.3f;

	const hkReal vehicleTopSpeed = 130.0f; 	 
	const hkReal wheelRadius = 0.4f;
	const hkReal maxEngineRpm = 7500.0f;
	transmission.m_primaryTransmissionRatio = hkpVehicleDefaultTransmission::calculatePrimaryTransmissionRatio( vehicleTopSpeed,
																												wheelRadius,
																												maxEngineRpm,
																												transmission.m_gearsRatio[ numGears - 1 ] );
}

void VehicleSetup::setupComponent( const hkpVehicleData& data, hkpVehicleDefaultBrake& brake )
{
	brake.m_wheelBrakingProperties.setSize( data.m_numWheels );

	const float bt = 1500.0f;
	brake.m_wheelBrakingProperties[0].m_maxBreakingTorque = bt;
	brake.m_wheelBrakingProperties[1].m_maxBreakingTorque = bt;
	brake.m_wheelBrakingProperties[2].m_maxBreakingTorque = bt;
	brake.m_wheelBrakingProperties[3].m_maxBreakingTorque = bt;

	// Handbrake is attached to rear wheels only.
	brake.m_wheelBrakingProperties[0].m_isConnectedToHandbrake = false;
	brake.m_wheelBrakingProperties[1].m_isConnectedToHandbrake = false;
	brake.m_wheelBrakingProperties[2].m_isConnectedToHandbrake = true;
	brake.m_wheelBrakingProperties[3].m_isConnectedToHandbrake = true;
	brake.m_wheelBrakingProperties[0].m_minPedalInputToBlock = 0.9f;
	brake.m_wheelBrakingProperties[1].m_minPedalInputToBlock = 0.9f;
	brake.m_wheelBrakingProperties[2].m_minPedalInputToBlock = 0.9f;
	brake.m_wheelBrakingProperties[3].m_minPedalInputToBlock = 0.9f;
	brake.m_wheelsMinTimeToBlock = 1000.0f;
}

void VehicleSetup::setupComponent( const hkpVehicleData& data, hkpVehicleDefaultSuspension& suspension ) 
{
	suspension.m_wheelParams.setSize( data.m_numWheels );
	suspension.m_wheelSpringParams.setSize( data.m_numWheels );

	suspension.m_wheelParams[0].m_length = 0.35f;
	suspension.m_wheelParams[1].m_length = 0.35f;
	suspension.m_wheelParams[2].m_length = 0.35f;
	suspension.m_wheelParams[3].m_length = 0.35f;

	const float str = 50.0f;
	suspension.m_wheelSpringParams[0].m_strength = str;
	suspension.m_wheelSpringParams[1].m_strength = str;
	suspension.m_wheelSpringParams[2].m_strength = str;
	suspension.m_wheelSpringParams[3].m_strength = str;

	const float wd = 3.0f; 
	suspension.m_wheelSpringParams[0].m_dampingCompression = wd;
	suspension.m_wheelSpringParams[1].m_dampingCompression = wd;
	suspension.m_wheelSpringParams[2].m_dampingCompression = wd;
	suspension.m_wheelSpringParams[3].m_dampingCompression = wd;

	suspension.m_wheelSpringParams[0].m_dampingRelaxation = wd;
	suspension.m_wheelSpringParams[1].m_dampingRelaxation = wd;
	suspension.m_wheelSpringParams[2].m_dampingRelaxation = wd;
	suspension.m_wheelSpringParams[3].m_dampingRelaxation = wd;
	
	//
	// NB: The hardpoints MUST be positioned INSIDE the chassis.
	//
	{
		const hkReal hardPointFrontX = 1.3f;
		const hkReal hardPointBackX = -1.1f;
		const hkReal hardPointY = -0.05f;
		const hkReal hardPointZ = 1.1f;

		suspension.m_wheelParams[0].m_hardpointChassisSpace.set ( hardPointFrontX, hardPointY, -hardPointZ); 
		suspension.m_wheelParams[1].m_hardpointChassisSpace.set ( hardPointFrontX, hardPointY,  hardPointZ); 
		suspension.m_wheelParams[2].m_hardpointChassisSpace.set ( hardPointBackX, hardPointY, -hardPointZ); 
		suspension.m_wheelParams[3].m_hardpointChassisSpace.set ( hardPointBackX, hardPointY,  hardPointZ); 
	}

	const hkVector4 downDirection( 0.0f, -1.0f, 0.0f );
	suspension.m_wheelParams[0].m_directionChassisSpace = downDirection;
	suspension.m_wheelParams[1].m_directionChassisSpace = downDirection;
	suspension.m_wheelParams[2].m_directionChassisSpace = downDirection;
	suspension.m_wheelParams[3].m_directionChassisSpace = downDirection;
}

void VehicleSetup::setupComponent( const hkpVehicleData& data, hkpVehicleDefaultAerodynamics& aerodynamics ) 
{
	aerodynamics.m_airDensity = 1.3f;
	// In m^2.
	aerodynamics.m_frontalArea = 1.0f;	

	aerodynamics.m_dragCoefficient = 0.7f;
	aerodynamics.m_liftCoefficient = -0.3f;

	// Extra gavity applies in world space (independent of m_chassisCoordinateSystem).
	aerodynamics.m_extraGravityws.set( 0.0f, -5.0f, 0.0f); 
}

void VehicleSetup::setupComponent( const hkpVehicleData& data, hkpVehicleDefaultVelocityDamper& velocityDamper )
{
	// Caution: setting negative damping values will add energy to system. 
	// Setting the value to 0 will not affect the angular velocity. 

	// Damping the change of the chassis angular velocity when below m_collisionThreshold. 
	// This will affect turning radius and steering.
	velocityDamper.m_normalSpinDamping    = 0.0f; 

	// Positive numbers dampen the rotation of the chassis and 
	// reduce the reaction of the chassis in a collision. 
	velocityDamper.m_collisionSpinDamping = 4.0f; 

	// The threshold in m/s at which the algorithm switches from 
	// using the normalSpinDamping to the collisionSpinDamping. 	
	velocityDamper.m_collisionThreshold   = 1.0f; 
}

void VehicleSetup::setupWheelCollide( const hkpWorld* world, const hkpVehicleInstance& vehicle, hkpVehicleRayCastWheelCollide& wheelCollide )
{
	// Set the wheels to have the same collision filter info as the chassis.
	wheelCollide.m_wheelCollisionFilterInfo = vehicle.getChassis()->getCollisionFilterInfo();
}

void VehicleSetup::setupTyremarks( const hkpVehicleData& data, hkpTyremarksInfo& tyreMarks ) 
{
	tyreMarks.m_minTyremarkEnergy = 100.0f;
	tyreMarks.m_maxTyremarkEnergy  = 1000.0f;
}











hkpConvexVerticesShape* createCarChassisShape()
{
	hkReal xSize = 1.75f;
	hkReal ySize = 0.25f;
	hkReal zSize = 1.1f;

	hkReal xBumper = 1.9f;
	hkReal yBumper = 0.15f;
	hkReal zBumper = 1.0f;

	hkReal xRoofFront = 0.4f;
	hkReal xRoofBack = -1.0f;
	hkReal yRoof = ySize + 0.45f;
	hkReal zRoof = 0.7f;

	hkReal xDoorFront = xRoofFront;
	hkReal xDoorBack = xRoofBack;
	hkReal yDoor = ySize;
	hkReal zDoor = zSize + 0.1f;

	int numVertices = 22;

	// 16 = 4 (size of "each float group", 3 for x,y,z, 1 for padding) * 4 (size of float).
	int stride = sizeof(hkReal) * 4;

	HK_ALIGN16(hkReal vertices[] )= { 
		xSize, ySize, zSize, 0.0f,		// v0
		xSize, ySize, -zSize, 0.0f,		// v1
		xSize, -ySize, zSize, 0.0f,		// v2
		xSize, -ySize, -zSize, 0.0f,	// v3
		-xSize, -ySize, zSize, 0.0f,	// v4
		-xSize, -ySize, -zSize, 0.0f,	// v5

		xBumper, yBumper, zBumper, 0.0f,	// v6
		xBumper, yBumper, -zBumper, 0.0f,	// v7
		-xBumper, yBumper, zBumper, 0.0f,	// v8
		-xBumper, yBumper, -zBumper, 0.0f,	// v9

		xRoofFront, yRoof, zRoof, 0.0f,		// v10
		xRoofFront, yRoof, -zRoof, 0.0f,	// v11
		xRoofBack, yRoof, zRoof, 0.0f,		// v12
		xRoofBack, yRoof, -zRoof, 0.0f,		// v13

		xDoorFront, yDoor, zDoor, 0.0f,		// v14
		xDoorFront, yDoor, -zDoor, 0.0f,	// v15
		xDoorFront, -yDoor, zDoor, 0.0f,	// v16
		xDoorFront, -yDoor, -zDoor, 0.0f,	// v17

		xDoorBack, yDoor, zDoor, 0.0f,		// v18
		xDoorBack, yDoor, -zDoor, 0.0f,		// v19
		xDoorBack, -yDoor, zDoor, 0.0f,		// v20
		xDoorBack, -yDoor, -zDoor, 0.0f,	// v21
	};
	
	//
	// SHAPE CONSTRUCTION.
	//
	
	hkStridedVertices		stridedVerts;
	stridedVerts.m_numVertices	=	numVertices;
	stridedVerts.m_striding		=	stride;
	stridedVerts.m_vertices		=	vertices;
	
	return new hkpConvexVerticesShape(stridedVerts);
}
















	hkpVehicleInstance* m_vehicle;
	hkpConvexVerticesShape* chassisShape = createCarChassisShape(); 
	//			hkpSphereShape* chassisShape = new hkpSphereShape(0.15f);
//	hkpRigidBody* chassis;
	hkpRigidBody* chassisRigidBody;

void Pojazd()
{

		{
		hkpRigidBodyCinfo chassisInfo;

		chassisInfo.m_mass = 750.0f;	
		chassisInfo.m_shape = chassisShape;
		chassisInfo.m_friction = 0.4f;

		chassisInfo.m_motionType = hkpMotion::MOTION_BOX_INERTIA;
		chassisInfo.m_position.set(0.0f, 1.0f, 0.0f);
		hkpInertiaTensorComputer::setShapeVolumeMassProperties(chassisInfo.m_shape,
										chassisInfo.m_mass,
										chassisInfo);

		chassisRigidBody = new hkpRigidBody(chassisInfo);

		// No longer need reference to shape as the hkpRigidBody holds one.
		chassisShape->removeReference();

		m_world->addEntity(chassisRigidBody);
	}

		//	createVehicle( chassisRigidBody );
	


	m_vehicle = new hkpVehicleInstance( chassisRigidBody );


	VehicleSetup setup;
	setup.buildVehicle( m_world, *m_vehicle );

	m_vehicle->addToWorld( m_world );

	m_world->addAction(m_vehicle);


	chassisRigidBody->removeReference();

}



























			




































//z game.h
const int WIDTH = 800; // default width of the window
const int HEIGHT = 600; // default height of the window

const float FRAME_PERIOD = 0.01667f; // ~60 fps

bool fullscreen = false;

int xOrigin = -1;
int yOrigin = -1;
float katY=0.0,deltakatY = 0.0;

float kat=0.0,deltakat = 0.0, ratio;
float x=0.0f,y=1.0f,z=3.0f;
float lx=0.0f,ly=0.0f,lz=0.0f;
int deltaRuch = 0;


// ---------------------------- More Constants ------------------------------ //

const float TORQUE_MULTIPLIER = 0.20f; // multiplier used for ball movement

const float VIEW_ANGLE = 50.0f; // field of view in the Y direction
const float NEAR_CLIPPING = 0.05f; // near clipping distance (5cm)
const float FAR_CLIPPING = 1000.0f; // far clipping distance (10m)

//const float ANGLE_LIMIT = 70.0f * HK_REAL_PI/180.0f; // 7 degrees limit

// ------------------------- Global GameData object ------------------------- //

// GameData structure
/* This structure represent all the data associated with a game instance,
 * this includes persistent data for operation of the Havok engine,
 * variables used for user input, and game status variables.
 */

	// Havok persistent objects
	

	hkpRigidBody* m_sphereBody;
	hkpRigidBody* m_sphereBody2;
	hkpRigidBody* m_sphereBody3;
	hkpRigidBody* m_ground;
	//hkpRigidBody* m_planeBody;
	#ifdef DEBUG
		hkVisualDebugger* m_vdb;
		hkpPhysicsContext* m_physicsContext;
	#endif

	// Sphere movement variables
	hkReal m_forward, m_strife; // variables for WASD movement
	

	// Function to reset everything in the GameData object
	void reset()
	{
		// reset everything
		m_sphereBody->setPositionAndRotation(hkVector4(0.0f, 0.6f, 0.0f), hkQuaternion::getIdentity());
		m_sphereBody->setLinearVelocity(hkVector4(0.0f, 0.0f, 0.0f));
		m_sphereBody->setAngularVelocity(hkVector4(0.0f, 0.0f, 0.0f));
		m_sphereBody2->setPositionAndRotation(hkVector4(0.3f, 1.0f, 0.0f), hkQuaternion::getIdentity());
		m_sphereBody2->setLinearVelocity(hkVector4(0.0f, 0.0f, 0.0f));
		m_sphereBody2->setAngularVelocity(hkVector4(0.0f, 0.0f, 0.0f));
		m_sphereBody3->setPositionAndRotation(hkVector4(-0.3f, 1.0f, 0.0f), hkQuaternion::getIdentity());
		m_sphereBody3->setLinearVelocity(hkVector4(0.0f, 0.0f, 0.0f));
		m_sphereBody3->setAngularVelocity(hkVector4(0.0f, 0.0f, 0.0f));
		m_forward = 0.0f;
		m_strife = 0.0f;
	}






// -------------------------- Graphics Utilities ---------------------------- //



	void mouseMove(int x, int y) {


		deltakat = (x - xOrigin) * 0.0001f;
    	deltakatY = (-y + yOrigin) * 0.0001f;

}

void mouseButton(int button, int state, int x, int y) {

	// only start motion if the left button is pressed
	if (button == GLUT_LEFT_BUTTON) {

		// when the button is released
		if (state == GLUT_UP) {
			kat += deltakat;
			xOrigin = -1;
			yOrigin = -1;
			deltakat = 0;
    	    deltakatY = 0;
		}
		else  {// state = GLUT_DOWN
			xOrigin = x;
			yOrigin = y;
		}
	}
}














void zorientujMnie (float ang) {
     
     lx = sin(ang);
     //ly = sin(ang);
     lz = -cos(ang);
     glLoadIdentity();
     gluLookAt(x, y, z, x+lx, y+ly, z+lz, 
                        0.0f, 1.0f, 0.0f);     
     }
     
     
void zorientujMnieY (float ang) {
     
     //lx = sin(ang);
     ly = tan(ang);
     //lz = -cos(ang);
     glLoadIdentity();
     gluLookAt(x, y, z, x+lx, y+ly, z+lz, 
                        0.0f, 1.0f, 0.0f);     
     }
     
     
     
void PlaskiRuch(int i) {
     x = x + i*(lx)*0.1;
     z = z + i*(lz)*0.1;
     glLoadIdentity();
     gluLookAt(x, y, z,
                  x + lx, y + ly, z + lz,
                  0.0f,1.0f,0.0f);
     }

















// Initialize OpenGL
static void initGraphics()
{
	glClearColor(0.0f, 0.0f, 0.0f, 1.0f);
	glCullFace(GL_BACK);
	glEnable(GL_CULL_FACE);
	glEnable(GL_LIGHTING);
	glShadeModel(GL_SMOOTH);
	glEnable(GL_DEPTH_TEST);
	glEnable (GL_BLEND);
	glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
	const GLfloat modelAmbient[] = {0.0f, 0.0f, 0.0f, 1.0f};
	glLightModelfv(GL_LIGHT_MODEL_AMBIENT, modelAmbient);
	const GLfloat lightAmbient[] = {0.4f, 0.4f, 0.4f, 0.0f};
	glLightfv(GL_LIGHT0, GL_AMBIENT, lightAmbient);
	const GLfloat lightDiffuse[] = {1.0f, 1.0f, 1.0f, 1.0f};
	glLightfv(GL_LIGHT0, GL_DIFFUSE, lightDiffuse);
	const GLfloat lightSpecular[] = {1.0f, 1.0f, 1.0f, 1.0f};
	glLightfv(GL_LIGHT0, GL_SPECULAR, lightSpecular);
	glEnable(GL_LIGHT0);

	const GLfloat lightPosition[] = {0.0f, 1.0f, 0.0f, 1.0f};
	glLightfv(GL_LIGHT0, GL_POSITION, lightPosition);
}

// Setup OpenGL for scene rendering
static void beginSceneRendering()
{
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(
		VIEW_ANGLE,
		static_cast<double>(glutGet(GLUT_WINDOW_WIDTH))/static_cast<double>(glutGet(GLUT_WINDOW_HEIGHT)), // aspect ratio
		NEAR_CLIPPING,
		FAR_CLIPPING);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	 	     gluLookAt(x, y, z, x+lx, y+ly, z+lz, 
                        0.0f, 1.0f, 0.0f);
	//gluLookAt(x=x+lx, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f);



if (deltaRuch)
   {PlaskiRuch(deltaRuch);}
if (deltakat) {
   kat += deltakat;
   zorientujMnie(kat);
}
 if (deltakatY) {
   katY += deltakatY;
   zorientujMnieY(katY);
}  


 




}

// Draw a sphere (after beginSceneRendering())
static void drawSphere(const hkVector4& position, const hkQuaternion& quaternion)
{
	glPushMatrix();
		const GLfloat diffuseColor[] = {0.6f, 0.6f, 0.6f, 1.0f};
		glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, diffuseColor);
		const GLfloat specularColor[] = {1.0f, 1.0f, 1.0f, 1.0f};
		glMaterialfv(GL_FRONT, GL_SPECULAR, specularColor);
		glMaterialf(GL_FRONT, GL_SHININESS, 60.0f);
		glTranslatef(position.getComponent(0), position.getComponent(1), position.getComponent(2));


				if(quaternion.hasValidAxis())
		{
		hkReal angle = quaternion.getAngle() * 180.0f / HK_REAL_PI;
			hkVector4 axis;
			quaternion.getAxis(axis);
			glRotatef(angle, axis.getSimdAt(0), axis.getSimdAt(1), axis.getSimdAt(2));
		}



		glutSolidSphere(0.07f, 50, 50);
	glPopMatrix();
	
}

static void drawSphere2(const hkVector4& position, const hkQuaternion& quaternion)
{
	glPushMatrix();
		const GLfloat diffuseColor[] = {1.0f, 0.0f, 0.0f, 1.0f};
		glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, diffuseColor);
		const GLfloat specularColor[] = {1.0f, 1.0f, 1.0f, 1.0f};
		glMaterialfv(GL_FRONT, GL_SPECULAR, specularColor);
		glMaterialf(GL_FRONT, GL_SHININESS, 60.0f);




		glTranslatef(position.getComponent(0), position.getComponent(1), position.getComponent(2));

		if(quaternion.hasValidAxis())
		{
		hkReal angle = quaternion.getAngle() * 180.0f / HK_REAL_PI;
			hkVector4 axis;
			quaternion.getAxis(axis);
			glRotatef(angle, axis.getSimdAt(0), axis.getSimdAt(1), axis.getSimdAt(2));
		}




		glutSolidSphere(0.10f, 10, 10);
	glPopMatrix();
}

static void drawSphere3(const hkVector4& position, const hkQuaternion& quaternion)
{
	glPushMatrix();
		const GLfloat diffuseColor[] = {0.0f, 1.0f, 0.0f, 1.0f};
		glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, diffuseColor);
		const GLfloat specularColor[] = {1.0f, 1.0f, 1.0f, 1.0f};
		glMaterialfv(GL_FRONT, GL_SPECULAR, specularColor);
		glMaterialf(GL_FRONT, GL_SHININESS, 60.0f);
		glTranslatef(position.getComponent(0), position.getComponent(1), position.getComponent(2));




				if(quaternion.hasValidAxis())
		{
		hkReal angle = quaternion.getAngle() * 180.0f / HK_REAL_PI;
			hkVector4 axis;
			quaternion.getAxis(axis);
			glRotatef(angle, axis.getSimdAt(0), axis.getSimdAt(1), axis.getSimdAt(2));
		}




		glutSolidSphere(0.15f, 50, 50);
	glPopMatrix();
}







static void drawPlane(const hkVector4& position)
{
	glPushMatrix();
		const GLfloat specularColor[] = {0.0f, 0.0f, 0.0f, 1.0f};
		glMaterialfv(GL_FRONT, GL_SPECULAR, specularColor);
		
				const GLfloat diffuseColor[] = {0.0f, 0.0f, 1.0f, 1.0f};
				glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, diffuseColor);
				
				glTranslatef(position.getComponent(0), position.getComponent(1), position.getComponent(2));
				glScalef(2.0f, 4.00f, 2.0f);
				glutSolidCube(1.0f);

	glPopMatrix();
}




// Draw text on screen (after the scene has been rendered)
static void drawText(const char* string)
{
	glDisable(GL_DEPTH_TEST);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
    glOrtho( 0.0, glutGet( GLUT_WINDOW_WIDTH ), 
	         0.0, glutGet( GLUT_WINDOW_HEIGHT ), 
	         -1.0, 1.0 );
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	glRasterPos2i( 10, glutGet( GLUT_WINDOW_HEIGHT ) - 30 );
	glColor4f(0.0f, 0.0f, 0.0f, 1.0f);
	//glutBitmapString(GLUT_BITMAP_HELVETICA_18, reinterpret_cast<const unsigned char*>(string));
	glEnable(GL_DEPTH_TEST);
}

// Deinitialize OpenGL (no-op)
static void deinitGraphics()
{}

// --------------------------- Error Reporting ------------------------------ //

// Havok error reporting function
static inline void HK_CALL errorReport(const char* msg, void* userContext)
{
	std::cerr << msg << std::endl;
}


// Initialization function
void OnInit() {
	// Initialize Havok
	// The frameinfo buffer must NOT be zero if physics is being used (it is the solver buffer)
#ifdef DEBUG
	hkMemoryRouter* memoryRouter = hkMemoryInitUtil::initChecking( hkMallocAllocator::m_defaultMallocAllocator, hkMemorySystem::FrameInfo(1024*1024) );
#else
	hkMemoryRouter* memoryRouter = hkMemoryInitUtil::initDefault( hkMallocAllocator::m_defaultMallocAllocator, hkMemorySystem::FrameInfo(1024*1024) );
#endif

	hkBaseSystem::init( memoryRouter, errorReport );
	{
		// Create the simulation world
		{
			hkpWorldCinfo worldInfo;
		worldInfo.m_collisionTolerance = 0.1f;
			worldInfo.m_gravity.set(0.0f,-9.8f,0.0f);
			worldInfo.setBroadPhaseWorldSize(10000.0f);
		worldInfo.setupSolverInfo(hkpWorldCinfo::SOLVER_TYPE_4ITERS_MEDIUM);
			m_world = new hkpWorld(worldInfo);
				
			// This is needed to detect collisions
			hkpAgentRegisterUtil::registerAllAgents( m_world->getCollisionDispatcher() );
		}
#ifdef DEBUG
		// Connect to the visual debugger
		{
			m_physicsContext = new hkpPhysicsContext();
			m_physicsContext->addWorld( m_world ); // add all worlds as you have
			hkpPhysicsContext::registerAllPhysicsProcesses();
			hkArray<hkProcessContext*> contexts;
			contexts.pushBack( m_physicsContext );  
			m_vdb = new hkVisualDebugger( contexts );
			m_vdb->serve();
		}
#endif
		// Create a sphere
		{
			hkpSphereShape* sphere = new hkpSphereShape(0.07f);
			// convex radius for spheres is exactly the sphere radius
			hkpRigidBodyCinfo rigidBodyInfo;
			rigidBodyInfo.m_shape = sphere;
			rigidBodyInfo.m_motionType = hkpMotion::MOTION_DYNAMIC;
			hkpInertiaTensorComputer::setShapeVolumeMassProperties(
				sphere, 1.0f, rigidBodyInfo);
			rigidBodyInfo.m_position.set(0.0f, 0.6f, 0.0f);
			rigidBodyInfo.m_friction = 1.0f;
			rigidBodyInfo.m_restitution = 0.2f;

			m_sphereBody = new hkpRigidBody(rigidBodyInfo);
			sphere->removeReference();
		
			m_world->addEntity(m_sphereBody);
		}


		// Create a sphere2
		{
			hkpSphereShape* sphere2 = new hkpSphereShape(0.10f);
			// convex radius for spheres is exactly the sphere radius
			hkpRigidBodyCinfo rigidBodyInfo2;
			rigidBodyInfo2.m_shape = sphere2;
			rigidBodyInfo2.m_motionType = hkpMotion::MOTION_DYNAMIC;
			hkpInertiaTensorComputer::setShapeVolumeMassProperties(
				sphere2, 0.1f, rigidBodyInfo2);
			rigidBodyInfo2.m_position.set(0.3f, 1.0f, 0.0f);
			rigidBodyInfo2.m_friction = 0.1f;
			rigidBodyInfo2.m_restitution = 0.9f;

			m_sphereBody2 = new hkpRigidBody(rigidBodyInfo2);
			sphere2->removeReference();
		
			m_world->addEntity(m_sphereBody2);
		}


				// Create a sphere3
		{
			hkpSphereShape* sphere3 = new hkpSphereShape(0.15f);
			// convex radius for spheres is exactly the sphere radius
			hkpRigidBodyCinfo rigidBodyInfo3;
			rigidBodyInfo3.m_shape = sphere3;
			rigidBodyInfo3.m_motionType = hkpMotion::MOTION_DYNAMIC;
			hkpInertiaTensorComputer::setShapeVolumeMassProperties(
				sphere3, 2.0f, rigidBodyInfo3);
			rigidBodyInfo3.m_position.set(-0.3f, 1.0f, 0.0f);
			rigidBodyInfo3.m_friction = 0.1f;
			rigidBodyInfo3.m_restitution = 0.1f;

			m_sphereBody3 = new hkpRigidBody(rigidBodyInfo3);
			sphere3->removeReference();
		
			m_world->addEntity(m_sphereBody3);
		}




		//ziemia


			{
		hkVector4 groundRadii( 1.0f, 2.0f, 1.0f );
		hkpConvexShape* shape = new hkpBoxShape( groundRadii , 0 );

		hkpRigidBodyCinfo ci;

		ci.m_shape = shape;
		ci.m_motionType = hkpMotion::MOTION_FIXED;
		ci.m_position = hkVector4( 0.0f, -2.0f, 0.0f );
		ci.m_qualityType = HK_COLLIDABLE_QUALITY_FIXED;
		m_ground = new hkpRigidBody( ci );
		m_world->addEntity(m_ground)->removeReference();
		shape->removeReference();
	}

	//hkVector4 groundPos( 0.0f, 0.0f, 0.0f );
	//hkVector4 posy = groundPos;


			Pojazd();
		


	}

	// initialize graphics
	initGraphics();
}







// ------------------------ Frame Rendering Function ------------------------ //

// Render the whole game scene
static void renderScene()
{
	beginSceneRendering();









		drawSphere(m_sphereBody->getPosition(),m_sphereBody->getRotation());
		drawSphere2(m_sphereBody2->getPosition(),m_sphereBody2->getRotation());
		drawSphere3(m_sphereBody3->getPosition(),m_sphereBody3->getRotation());
		//drawPlane(m_planeBody->getRotation()); 
		drawPlane(m_ground->getPosition());
	//endSceneRendering();
}

// Frame rendering function
void OnFrame() {


		
		
			{
				
				m_sphereBody->applyTorque(FRAME_PERIOD, hkVector4(-m_forward, 0.0f, -m_strife));

				m_world->stepDeltaTime(FRAME_PERIOD);
				#ifdef DEBUG
					m_vdb->step(FRAME_PERIOD);
				#endif

				renderScene();

				const hkVector4& spherePos = m_sphereBody->getPosition();
				const hkVector4& spherePos2 = m_sphereBody2->getPosition();
			}

}

// ----------------------- Deinitialization Function ------------------------ //

void OnExit() {
	deinitGraphics();

	// delete havok entities
	{
		//m_planeBody->removeReference();
		m_sphereBody->removeReference();
		m_sphereBody2->removeReference();
		m_sphereBody3->removeReference();
		m_ground->removeReference();
#ifdef DEBUG
		m_vdb->removeReference();
		m_physicsContext->removeReference();
#endif
		m_world->removeReference();
	}
	hkBaseSystem::quit();
	hkMemoryInitUtil::quit();
}

// ------------------- Standard Key Management Function --------------------- //

// Key manager function
void KeyManager(unsigned char key, int x, int y) {
	
	switch(key) {
		case 'w': // move forward
		case 'W':
			m_forward = TORQUE_MULTIPLIER;
			break;
		case 's': // move backwards
		case 'S':
			m_forward = -TORQUE_MULTIPLIER;
			break;
		case 'd': // strife right
		case 'D':
			m_strife = TORQUE_MULTIPLIER;
			break;
		case 'a': // strife left
		case 'A':
			m_strife = -TORQUE_MULTIPLIER;
			break;
		case 'c': 
		case 'C': // tabulation: toggle fullscreen
			if(!fullscreen) {
				glutFullScreen();
				fullscreen = true;
			} else {
				glutPositionWindow(10,10);
				glutReshapeWindow(WIDTH,HEIGHT);
				fullscreen = false;
			}
			break;
		case 'r':
		case 'R':	//restart
			reset();

			break;
	case 'o':
	case 'O': deltaRuch = 1;break;
	case 'L':

	case 'l':
        deltaRuch = -1;break;
	case 'k':
	case 'K':deltakat = -0.01f;break;
	case ';':
	case ':':
        deltakat = 0.01f;break;






		}



	     if (key == 27)
        exit(0);
        





	return;
}

// ------------------------ Key Up Manager Function ------------------------- //

// Key up manager function
void KeyUpManager(unsigned char key, int x, int y) {
	switch(key) {
	case 'w':
	case 'W':
	case 's':
	case 'S':
		m_forward = 0.0f;
		break;
	case 'd':
	case 'D':
	case 'a':
	case 'A':
		m_strife = 0.0f;
		break;

	case 'O':
	case 'L':
	case 'o':
	case 'l':
        deltaRuch = 0;break;
	case 'k':
	case 'K':
	case ';':
	case ':':
        deltakat = 0.0f;break;
        

	}




}

// ----------------------- Timer Management Function ------------------------ //

// Timer manager
void TimerManager(int) {
	// post a new display operation
	glutPostRedisplay();
		
	glutTimerFunc(static_cast<unsigned>(FRAME_PERIOD*1000.0f), TimerManager, 0);
}

// ------------------- Window Reshape Management Function ------------------- //

// Window resize manager
void ResizeManager(int width, int height) {
	glViewport(0, 0, width, height);




}

#include <Common/Base/keycode.cxx>

// we're not using any product apart from Havok Physics.
#undef HK_FEATURE_PRODUCT_AI
#undef HK_FEATURE_PRODUCT_ANIMATION
#undef HK_FEATURE_PRODUCT_CLOTH
#undef HK_FEATURE_PRODUCT_DESTRUCTION
#undef HK_FEATURE_PRODUCT_BEHAVIOR

// Also we're not using any serialization/versioning so we don't need any of these.

#define HK_EXCLUDE_FEATURE_SerializeDeprecatedPre700
#define HK_EXCLUDE_FEATURE_RegisterVersionPatches
#define HK_EXCLUDE_FEATURE_MemoryTracker

#include <Common/Base/Config/hkProductFeatures.cxx>



void idle()
{
 //glutPostRedisplay();

}




// Drawing function
/* Uses the OnFrame() function to execute all scene-specific
 * actions, and also perform the swap buffers operation.
 */
void DrawGLScene(void) {
    OnFrame();
    glutSwapBuffers();
}

// Main function
int main(int argc, char **argv) {
	
    //int win;
    // FreeGLUT init
    glutInit(&argc,argv);
    glutInitDisplayMode(GLUT_RGBA|GLUT_DOUBLE|
        GLUT_MULTISAMPLE|GLUT_DEPTH);
    glutInitWindowSize(WIDTH,HEIGHT);
    glutInitWindowPosition(10,10);
    glutCreateWindow("HavokOGL");
    // initialize the scene
    OnInit();
	glutIgnoreKeyRepeat(1);
    glutDisplayFunc(DrawGLScene);
	glutIdleFunc(idle);
    glutReshapeFunc(ResizeManager);
    glutKeyboardFunc(KeyManager);
    glutKeyboardUpFunc(KeyUpManager);
   	glutMouseFunc(mouseButton);
	glutMotionFunc(mouseMove);

   // glutSetOption(GLUT_ACTION_ON_WINDOW_CLOSE,
   //     GLUT_ACTION_CONTINUE_EXECUTION);
    glutTimerFunc(static_cast<unsigned>(FRAME_PERIOD*1000.0f), TimerManager, 0);
    glutMainLoop();
    OnExit();
    return 0;
}