//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//  * Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//  * Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
//  * Neither the name of NVIDIA CORPORATION nor the names of its
//    contributors may be used to endorse or promote products derived
//    from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ``AS IS'' AND ANY
// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// Copyright (c) 2008-2019 NVIDIA Corporation. All rights reserved.
// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  

// ****************************************************************************
// This snippet illustrates how to use contact modification and sweeps to allow wheels to 
// collide and react more naturally with the scene.  In particular, the snippet shows how to 
// use contact modification and ccd contact modification to select or ignore rigid body contact 
// points between a shape representing a wheel and any other shape in the scene.  The snippet 
// also demonstrates the use of suspension sweeps instead of suspension raycasts.

// The snippet creates various static capsules with different radii, a ground plane, and a 
// vehicle.  The capsules are configured to be drivable surfaces.  Additionally, the 
// capsule and wheel shapes are configured with simulation filter data so that 
// they 
//  (i)  collide with each other with discrete collision detection
// (ii)  collide with each other with continuous collision detection (CCD)
//(iii)  trigger a contact modification callback  
// (iv)  trigger a ccd contact modification callback

// The contact modification callback is implemented with the class WheelContactModifyCallback.  
// The function WheelContactModifyCallback::onContactModify identifies shape pairs that contain
// a wheel.  Contact points for the shape pair are ignored or accepted with the SDK function 
// PxVehicleModifyWheelContacts.  CCD contact modification is implemented with the class 
// WheelCCDContactModifyCallback.  The function WheelCCDContactModifyCallback::onContactModify
// performs exactly the same role as WheelContactModifyCallback::onContactModify

// The threshold values POINT_REJECT_ANGLE and NORMAL_REJECT_ANGLE can be tuned
// to modify the conditions under which wheel contact points are ignored or accepted.

// It is a good idea to record and playback with pvd (PhysX Visual Debugger).
// ****************************************************************************

#include "PxPhysicsAPI.h"
#include <ctype.h>

#include "vehicle/PxVehicleUtil.h"
#include "../snippetvehiclecommon/SnippetVehicleSceneQuery.h"
#include "../snippetvehiclecommon/SnippetVehicleFilterShader.h"
#include "../snippetvehiclecommon/SnippetVehicleTireFriction.h"
#include "../snippetvehiclecommon/SnippetVehicleCreate.h"
#include "../snippetcommon/SnippetPVD.h"


#include <iostream>

using namespace physx;
using namespace snippetvehicle;

PxDefaultAllocator		gAllocator;
PxDefaultErrorCallback	gErrorCallback;

PxFoundation*			gFoundation = NULL;
PxPhysics*				gPhysics	= NULL;

PxDefaultCpuDispatcher*	gDispatcher = NULL;
PxScene*				gScene		= NULL;

PxCooking*				gCooking	= NULL;

PxMaterial*				gMaterial	= NULL;

PxPvd*                  gPvd = NULL;

VehicleSceneQueryData*	gVehicleSceneQueryData = NULL;
PxBatchQuery*			gBatchQuery = NULL;

PxVehicleDrivableSurfaceToTireFrictionPairs* gFrictionPairs = NULL;

#define NUM_VEHICLES 2
PxRigidStatic*			gGroundPlane = NULL;
PxVehicleDrive4W*		gVehicle4W[NUM_VEHICLES] = { NULL, NULL };
PxVehicleDrive4W*		gTrailer[NUM_VEHICLES] = { NULL, NULL };
PxRigidDynamic*			gCab[NUM_VEHICLES] = { NULL, NULL };

ActorUserData gActorUserData[NUM_VEHICLES];
ShapeUserData gShapeUserDatas[NUM_VEHICLES][PX_MAX_NB_WHEELS];

ActorUserData gTrailerActorUserData[NUM_VEHICLES];
ShapeUserData gTrailerShapeUserDatas[NUM_VEHICLES][PX_MAX_NB_WHEELS];

ActorUserData gCabinActorUserData[NUM_VEHICLES];
ShapeUserData gCabinShapeUserDatas[NUM_VEHICLES][PX_MAX_NB_WHEELS];

const PxF32 xCoordVehicleStarts[NUM_VEHICLES] = { 0.0f, 20.0f };

//Angle thresholds used to categorize contacts as suspension contacts or rigid body contacts.
#define POINT_REJECT_ANGLE PxPi/4.0f
#define NORMAL_REJECT_ANGLE PxPi/4.0f

//Contact modification values.
#define WHEEL_TANGENT_VELOCITY_MULTIPLIER 0.1f
#define MAX_IMPULSE PX_MAX_F32

//PhysX Vehicles support blocking and non-blocking sweeps.
//Experiment with this define to switch between the two regimes.
#define BLOCKING_SWEEPS 0

//Define the maximum acceleration for dynamic bodies under the wheel.
#define MAX_ACCELERATION 50.0f

//Blocking sweeps require sweep hit buffers for just 1 hit per wheel.
//Non-blocking sweeps require more hits per wheel because they return all touches on the swept shape.
#if BLOCKING_SWEEPS
PxU16					gNbQueryHitsPerWheel = 1;
#else
PxU16					gNbQueryHitsPerWheel = 8;
#endif

static PxVec3 gChassisMeshTransform(0,0,0);

////////////////////////////////////////////////////////////////
//WHEEL CENTRE OFFSETS FROM CENTRE OF CHASSIS RENDER MESH AABB
//OF 4-WHEELED VEHICLE
////////////////////////////////////////////////////////////////

static PxVec3 gWheelCentreOffsets4[4];

////////////////////////////////////////////////////////////////
//CONVEX HULL OF RENDER MESH FOR CHASSIS AND WHEELS OF
//4-WHEELED VEHICLE
////////////////////////////////////////////////////////////////

static PxConvexMesh* gChassisConvexMesh[NUM_VEHICLES]={NULL};
static PxConvexMesh* gWheelConvexMeshes4[4]={NULL,NULL,NULL,NULL};


//#define CHECK_MSG(exp, msg) (!!(exp) || (physx::shdfnd::getFoundation().error(physx::PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__, msg), 0) )

enum
{
        SAMPLEVEHICLE_DRIVABLE_SURFACE = 0xffff0000,
        SAMPLEVEHICLE_UNDRIVABLE_SURFACE = 0x0000ffff
};

void SampleVehicleSetupDrivableShapeQueryFilterData(PxFilterData* qryFilterData)
{
        //CHECK_MSG(0==qryFilterData->word3, "word3 is reserved for filter data for vehicle raycast queries");
        qryFilterData->word3 = (PxU32)SAMPLEVEHICLE_DRIVABLE_SURFACE;
}

void SampleVehicleSetupNonDrivableShapeQueryFilterData(PxFilterData* qryFilterData)
{
        //CHECK_MSG(0==qryFilterData->word3, "word3 is reserved for filter data for vehicle raycast queries");
        qryFilterData->word3 = (PxU32)SAMPLEVEHICLE_UNDRIVABLE_SURFACE;
}

void SampleVehicleSetupVehicleShapeQueryFilterData(PxFilterData* qryFilterData)
{
        //CHECK_MSG(0==qryFilterData->word3, "word3 is reserved for filter data for vehicle raycast queries");
        qryFilterData->word3 = (PxU32)SAMPLEVEHICLE_UNDRIVABLE_SURFACE;
}

void computeWheelWidthsAndRadii(PxConvexMesh** wheelConvexMeshes, PxF32* wheelWidths, PxF32* wheelRadii)
{
        for(PxU32 i=0;i<4;i++)
        {
                const PxU32 numWheelVerts=wheelConvexMeshes[i]->getNbVertices();
                const PxVec3* wheelVerts=wheelConvexMeshes[i]->getVertices();
                PxVec3 wheelMin(PX_MAX_F32,PX_MAX_F32,PX_MAX_F32);
                PxVec3 wheelMax(-PX_MAX_F32,-PX_MAX_F32,-PX_MAX_F32);
                for(PxU32 j=0;j<numWheelVerts;j++)
                {
                        wheelMin.x=PxMin(wheelMin.x,wheelVerts[j].x);
                        wheelMin.y=PxMin(wheelMin.y,wheelVerts[j].y);
                        wheelMin.z=PxMin(wheelMin.z,wheelVerts[j].z);
                        wheelMax.x=PxMax(wheelMax.x,wheelVerts[j].x);
                        wheelMax.y=PxMax(wheelMax.y,wheelVerts[j].y);
                        wheelMax.z=PxMax(wheelMax.z,wheelVerts[j].z);
                }
                wheelWidths[i]=wheelMax.x-wheelMin.x;
                wheelRadii[i]=PxMax(wheelMax.y,wheelMax.z)*0.975f;
        }
}

PxVec3 computeChassisAABBDimensions(const PxConvexMesh* chassisConvexMesh)
{
        const PxU32 numChassisVerts=chassisConvexMesh->getNbVertices();
        const PxVec3* chassisVerts=chassisConvexMesh->getVertices();
        PxVec3 chassisMin(PX_MAX_F32,PX_MAX_F32,PX_MAX_F32);
        PxVec3 chassisMax(-PX_MAX_F32,-PX_MAX_F32,-PX_MAX_F32);
        for(PxU32 i=0;i<numChassisVerts;i++)
        {
                chassisMin.x=PxMin(chassisMin.x,chassisVerts[i].x);
                chassisMin.y=PxMin(chassisMin.y,chassisVerts[i].y);
                chassisMin.z=PxMin(chassisMin.z,chassisVerts[i].z);
                chassisMax.x=PxMax(chassisMax.x,chassisVerts[i].x);
                chassisMax.y=PxMax(chassisMax.y,chassisVerts[i].y);
                chassisMax.z=PxMax(chassisMax.z,chassisVerts[i].z);
        }
        const PxVec3 chassisDims=chassisMax-chassisMin;
        return chassisDims;
}

void createVehicle4WSimulationData
(const PxF32 chassisMass, PxConvexMesh* chassisConvexMesh,
 const PxF32 wheelMass, PxConvexMesh** wheelConvexMeshes, const PxVec3* wheelCentreOffsets,
 PxVehicleWheelsSimData& wheelsData, PxVehicleDriveSimData4W& driveData, PxVehicleChassisData& chassisData)
{
        //Extract the chassis AABB dimensions from the chassis convex mesh.
        std::cout << __LINE__ << ": passed" << std::endl;
        const PxVec3 chassisDims=computeChassisAABBDimensions(chassisConvexMesh);
        std::cout << __LINE__ << ": passed" << std::endl;

        //The origin is at the center of the chassis mesh.
        //Set the center of mass to be below this point and a little towards the front.
        const PxVec3 chassisCMOffset=PxVec3(0.0f,-chassisDims.y*0.5f+0.65f,0.25f);

        //Now compute the chassis mass and moment of inertia.
        //Use the moment of inertia of a cuboid as an approximate value for the chassis moi.
        PxVec3 chassisMOI
                ((chassisDims.y*chassisDims.y + chassisDims.z*chassisDims.z)*chassisMass/12.0f,
                 (chassisDims.x*chassisDims.x + chassisDims.z*chassisDims.z)*chassisMass/12.0f,
                 (chassisDims.x*chassisDims.x + chassisDims.y*chassisDims.y)*chassisMass/12.0f);
        //A bit of tweaking here.  The car will have more responsive turning if we reduce the
        //y-component of the chassis moment of inertia.
        chassisMOI.y*=0.8f;

        //Let's set up the chassis data structure now.
        chassisData.mMass=chassisMass;
        chassisData.mMOI=chassisMOI;
        chassisData.mCMOffset=chassisCMOffset;

        //Compute the sprung masses of each suspension spring using a helper function.
        PxF32 suspSprungMasses[4];
        PxVehicleComputeSprungMasses(4,wheelCentreOffsets,chassisCMOffset,chassisMass,1,suspSprungMasses);

        //Extract the wheel radius and width from the wheel convex meshes.
        PxF32 wheelWidths[4];
        PxF32 wheelRadii[4];
        computeWheelWidthsAndRadii(wheelConvexMeshes,wheelWidths,wheelRadii);

        //Now compute the wheel masses and inertias components around the axle's axis.
        //http://en.wikipedia.org/wiki/List_of_moments_of_inertia
        PxF32 wheelMOIs[4];
        for(PxU32 i=0;i<4;i++)
        {
                wheelMOIs[i]=0.5f*wheelMass*wheelRadii[i]*wheelRadii[i];
        }
        //Let's set up the wheel data structures now with radius, mass, and moi.
        PxVehicleWheelData wheels[4];
        for(PxU32 i=0;i<4;i++)
        {
                wheels[i].mRadius=wheelRadii[i];
                wheels[i].mMass=wheelMass;
                wheels[i].mMOI=wheelMOIs[i];
                wheels[i].mWidth=wheelWidths[i];
        }
        //Disable the handbrake from the front wheels and enable for the rear wheels
        wheels[PxVehicleDrive4WWheelOrder::eFRONT_LEFT].mMaxHandBrakeTorque=0.0f;
        wheels[PxVehicleDrive4WWheelOrder::eFRONT_RIGHT].mMaxHandBrakeTorque=0.0f;
        wheels[PxVehicleDrive4WWheelOrder::eREAR_LEFT].mMaxHandBrakeTorque=4000.0f;
        wheels[PxVehicleDrive4WWheelOrder::eREAR_RIGHT].mMaxHandBrakeTorque=4000.0f;
        //Enable steering for the front wheels and disable for the front wheels.
        wheels[PxVehicleDrive4WWheelOrder::eFRONT_LEFT].mMaxSteer=PxPi*0.3333f;
        wheels[PxVehicleDrive4WWheelOrder::eFRONT_RIGHT].mMaxSteer=PxPi*0.3333f;
        wheels[PxVehicleDrive4WWheelOrder::eREAR_LEFT].mMaxSteer=0.0f;
        wheels[PxVehicleDrive4WWheelOrder::eREAR_RIGHT].mMaxSteer=0.0f;

        //Let's set up the tire data structures now.
        //Put slicks on the front tires and wets on the rear tires.
        PxVehicleTireData tires[4];
        tires[PxVehicleDrive4WWheelOrder::eFRONT_LEFT].mType=TIRE_TYPE_WORN;
        tires[PxVehicleDrive4WWheelOrder::eFRONT_RIGHT].mType=TIRE_TYPE_WORN;
        tires[PxVehicleDrive4WWheelOrder::eREAR_LEFT].mType=TIRE_TYPE_WORN;
        tires[PxVehicleDrive4WWheelOrder::eREAR_RIGHT].mType=TIRE_TYPE_WORN;

        //Let's set up the suspension data structures now.
        PxVehicleSuspensionData susps[4];
        for(PxU32 i=0;i<4;i++)
        {
                susps[i].mMaxCompression=0.3f;
                susps[i].mMaxDroop=0.1f;
                susps[i].mSpringStrength=35000.0f;
                susps[i].mSpringDamperRate=4500.0f;
        }
        susps[PxVehicleDrive4WWheelOrder::eFRONT_LEFT].mSprungMass=suspSprungMasses[PxVehicleDrive4WWheelOrder::eFRONT_LEFT];
        susps[PxVehicleDrive4WWheelOrder::eFRONT_RIGHT].mSprungMass=suspSprungMasses[PxVehicleDrive4WWheelOrder::eFRONT_RIGHT];
        susps[PxVehicleDrive4WWheelOrder::eREAR_LEFT].mSprungMass=suspSprungMasses[PxVehicleDrive4WWheelOrder::eREAR_LEFT];
        susps[PxVehicleDrive4WWheelOrder::eREAR_RIGHT].mSprungMass=suspSprungMasses[PxVehicleDrive4WWheelOrder::eREAR_RIGHT];

        //Set up the camber.
        //Remember that the left and right wheels need opposite camber so that the car preserves symmetry about the forward direction.
        //Set the camber to 0.0f when the spring is neither compressed or elongated.
        const PxF32 camberAngleAtRest=0.0;
        susps[PxVehicleDrive4WWheelOrder::eFRONT_LEFT].mCamberAtRest=camberAngleAtRest;
        susps[PxVehicleDrive4WWheelOrder::eFRONT_RIGHT].mCamberAtRest=-camberAngleAtRest;
        susps[PxVehicleDrive4WWheelOrder::eREAR_LEFT].mCamberAtRest=camberAngleAtRest;
        susps[PxVehicleDrive4WWheelOrder::eREAR_RIGHT].mCamberAtRest=-camberAngleAtRest;
        //Set the wheels to camber inwards at maximum droop (the left and right wheels almost form a V shape)
        const PxF32 camberAngleAtMaxDroop=0.001f;
        susps[PxVehicleDrive4WWheelOrder::eFRONT_LEFT].mCamberAtMaxDroop=camberAngleAtMaxDroop;
        susps[PxVehicleDrive4WWheelOrder::eFRONT_RIGHT].mCamberAtMaxDroop=-camberAngleAtMaxDroop;
        susps[PxVehicleDrive4WWheelOrder::eREAR_LEFT].mCamberAtMaxDroop=camberAngleAtMaxDroop;
        susps[PxVehicleDrive4WWheelOrder::eREAR_RIGHT].mCamberAtMaxDroop=-camberAngleAtMaxDroop;
        //Set the wheels to camber outwards at maximum compression (the left and right wheels almost form a A shape).
        const PxF32 camberAngleAtMaxCompression=-0.001f;
        susps[PxVehicleDrive4WWheelOrder::eFRONT_LEFT].mCamberAtMaxCompression=camberAngleAtMaxCompression;
        susps[PxVehicleDrive4WWheelOrder::eFRONT_RIGHT].mCamberAtMaxCompression=-camberAngleAtMaxCompression;
        susps[PxVehicleDrive4WWheelOrder::eREAR_LEFT].mCamberAtMaxCompression=camberAngleAtMaxCompression;
        susps[PxVehicleDrive4WWheelOrder::eREAR_RIGHT].mCamberAtMaxCompression=-camberAngleAtMaxCompression;

        //We need to set up geometry data for the suspension, wheels, and tires.
        //We already know the wheel centers described as offsets from the actor center and the center of mass offset from actor center.
        //From here we can approximate application points for the tire and suspension forces.
        //Lets assume that the suspension travel directions are absolutely vertical.
        //Also assume that we apply the tire and suspension forces 30cm below the center of mass.
        PxVec3 suspTravelDirections[4]={PxVec3(0,-1,0),PxVec3(0,-1,0),PxVec3(0,-1,0),PxVec3(0,-1,0)};
        PxVec3 wheelCentreCMOffsets[4];
        PxVec3 suspForceAppCMOffsets[4];
        PxVec3 tireForceAppCMOffsets[4];
        for(PxU32 i=0;i<4;i++)
        {
                wheelCentreCMOffsets[i]=wheelCentreOffsets[i]-chassisCMOffset;
                suspForceAppCMOffsets[i]=PxVec3(wheelCentreCMOffsets[i].x,-0.3f,wheelCentreCMOffsets[i].z);
                tireForceAppCMOffsets[i]=PxVec3(wheelCentreCMOffsets[i].x,-0.3f,wheelCentreCMOffsets[i].z);
        }

        //Now add the wheel, tire and suspension data.
        for(PxU32 i=0;i<4;i++)
        {
                wheelsData.setWheelData(i,wheels[i]);
                wheelsData.setTireData(i,tires[i]);
                wheelsData.setSuspensionData(i,susps[i]);
                wheelsData.setSuspTravelDirection(i,suspTravelDirections[i]);
                wheelsData.setWheelCentreOffset(i,wheelCentreCMOffsets[i]);
                wheelsData.setSuspForceAppPointOffset(i,suspForceAppCMOffsets[i]);
                wheelsData.setTireForceAppPointOffset(i,tireForceAppCMOffsets[i]);
        }

        //Set the car to perform 3 sub-steps when it moves with a forwards speed of less than 5.0
        //and with a single step when it moves at speed greater than or equal to 5.0.
        wheelsData.setSubStepCount(5.0f, 3, 1);


        //Now set up the differential, engine, gears, clutch, and ackermann steering.

        //Diff
        PxVehicleDifferential4WData diff;
        diff.mType=PxVehicleDifferential4WData::eDIFF_TYPE_LS_4WD;
        driveData.setDiffData(diff);

        //Engine
        PxVehicleEngineData engine;
        engine.mPeakTorque=1500.0f;
        engine.mMaxOmega=600.0f;//approx 6000 rpm
        driveData.setEngineData(engine);

        //Gears
        PxVehicleGearsData gears;
        gears.mSwitchTime=0.5f;
        driveData.setGearsData(gears);

        //Clutch
        PxVehicleClutchData clutch;
        clutch.mStrength=10.0f;
        driveData.setClutchData(clutch);

        //Ackermann steer accuracy
        PxVehicleAckermannGeometryData ackermann;
        ackermann.mAccuracy=1.0f;
        ackermann.mAxleSeparation=wheelCentreOffsets[PxVehicleDrive4WWheelOrder::eFRONT_LEFT].z-wheelCentreOffsets[PxVehicleDrive4WWheelOrder::eREAR_LEFT].z;
        ackermann.mFrontWidth=wheelCentreOffsets[PxVehicleDrive4WWheelOrder::eFRONT_RIGHT].x-wheelCentreOffsets[PxVehicleDrive4WWheelOrder::eFRONT_LEFT].x;
        ackermann.mRearWidth=wheelCentreOffsets[PxVehicleDrive4WWheelOrder::eREAR_RIGHT].x-wheelCentreOffsets[PxVehicleDrive4WWheelOrder::eREAR_LEFT].x;
        driveData.setAckermannGeometryData(ackermann);

        std::cout << __LINE__ << ": passed" << std::endl;
}

void setupActor
(PxRigidDynamic* vehActor,
 const PxFilterData& vehQryFilterData,
 const PxGeometry** wheelGeometries, const PxTransform* wheelLocalPoses, const PxU32 numWheelGeometries, const PxMaterial* wheelMaterial, const PxFilterData& wheelCollFilterData,
 const PxGeometry** chassisGeometries, const PxTransform* chassisLocalPoses, const PxU32 numChassisGeometries, const PxMaterial* chassisMaterial, const PxFilterData& chassisCollFilterData,
 const PxVehicleChassisData& chassisData,
 PxPhysics* physics)
{
        PX_UNUSED(physics);
        //Add all the wheel shapes to the actor.
        for(PxU32 i=0;i<numWheelGeometries;i++)
        {
                PxShape* wheelShape=PxRigidActorExt::createExclusiveShape(*vehActor, *wheelGeometries[i],*wheelMaterial);
                wheelShape->setQueryFilterData(vehQryFilterData);
                wheelShape->setSimulationFilterData(wheelCollFilterData);
                wheelShape->setLocalPose(wheelLocalPoses[i]);
        }

        //Add the chassis shapes to the actor.
        for(PxU32 i=0;i<numChassisGeometries;i++)
        {
                PxShape* chassisShape=PxRigidActorExt::createExclusiveShape(*vehActor, *chassisGeometries[i],*chassisMaterial);
                chassisShape->setQueryFilterData(vehQryFilterData);
                chassisShape->setSimulationFilterData(chassisCollFilterData);
                chassisShape->setLocalPose(chassisLocalPoses[i]);
        }

        vehActor->setMass(chassisData.mMass);
        vehActor->setMassSpaceInertiaTensor(chassisData.mMOI);
        vehActor->setCMassLocalPose(PxTransform(chassisData.mCMOffset,PxQuat(PxIdentity)));
}
PxRigidDynamic* createVehicleActor6W(const PxVehicleChassisData& chassisData,PxConvexMesh** wheelConvexMeshes, PxConvexMesh* chassisConvexMesh, PxScene& scene, PxPhysics& physics, const PxMaterial& material)
{
        PX_UNUSED(scene);
        PxTransform ident=PxTransform(PxIdentity);

        //We need a rigid body actor for the vehicle.
        //Don't forget to add the actor the scene after setting up the associated vehicle.
        PxRigidDynamic* vehActor=physics.createRigidDynamic(PxTransform(PxIdentity));

        //We need to add wheel collision shapes, their local poses, a material for the wheels, and a simulation filter for the wheels.
        PxConvexMeshGeometry frontLeftWheelGeom(wheelConvexMeshes[0]);
        PxConvexMeshGeometry frontRightWheelGeom(wheelConvexMeshes[1]);
        PxConvexMeshGeometry rearLeftWheelGeom(wheelConvexMeshes[2]);
        PxConvexMeshGeometry rearRightWheelGeom(wheelConvexMeshes[3]);
        PxConvexMeshGeometry extraWheel0(wheelConvexMeshes[0]);
        PxConvexMeshGeometry extraWheel1(wheelConvexMeshes[1]);
        const PxGeometry* wheelGeoms[6]={&frontLeftWheelGeom,&frontRightWheelGeom,&rearLeftWheelGeom,&rearRightWheelGeom,&extraWheel0,&extraWheel1};
        const PxTransform wheelLocalPoses[6]={ident,ident,ident,ident,ident,ident};
        const PxMaterial& wheelMaterial=material;
        PxFilterData wheelCollFilterData;
        wheelCollFilterData.word0=COLLISION_FLAG_WHEEL;
        wheelCollFilterData.word1=COLLISION_FLAG_WHEEL_AGAINST;

        //We need to add chassis collision shapes, their local poses, a material for the chassis, and a simulation filter for the chassis.
        PxConvexMeshGeometry chassisConvexGeom(chassisConvexMesh);
        const PxGeometry* chassisGeoms[1]={&chassisConvexGeom};
        const PxTransform chassisLocalPoses[1]={ident};
        const PxMaterial& chassisMaterial=material;
        PxFilterData chassisCollFilterData;
        chassisCollFilterData.word0=COLLISION_FLAG_CHASSIS;
        chassisCollFilterData.word1=COLLISION_FLAG_CHASSIS_AGAINST;

        //Create a query filter data for the car to ensure that cars
        //do not attempt to drive on themselves.
        PxFilterData vehQryFilterData;
        SampleVehicleSetupVehicleShapeQueryFilterData(&vehQryFilterData);

        //Set up the physx rigid body actor with shapes, local poses, and filters.
        setupActor(
                vehActor,
                vehQryFilterData,
                wheelGeoms,wheelLocalPoses,6,&wheelMaterial,wheelCollFilterData,
                chassisGeoms,chassisLocalPoses,1,&chassisMaterial,chassisCollFilterData,
                chassisData,
                &physics);

        return vehActor;
}

#if 1
void createVehicle6WSimulationData
(const PxF32 chassisMass, const PxVec3* wheelCentreOffsets4, PxConvexMesh* chassisConvexMesh, PxConvexMesh** wheelConvexMeshes4,
 PxVehicleWheelsSimData& wheelsSimData6W, PxVehicleDriveSimData4W& driveSimData6W, PxVehicleChassisData& chassisData6W)
{
        //Start by constructing the simulation data for a 4-wheeled vehicle.
        PxVehicleWheelsSimData* wheelsSimData4W=PxVehicleWheelsSimData::allocate(4);
        PxVehicleDriveSimData4W driveData4W;
        PxVehicleChassisData chassisData4W;

        std::cout << __LINE__ << ": passed" << std::endl;

        createVehicle4WSimulationData(
                chassisMass,chassisConvexMesh,
                20.0f,wheelConvexMeshes4,wheelCentreOffsets4,
                *wheelsSimData4W,driveData4W,chassisData4W);

        std::cout << __LINE__ << ": passed" << std::endl;

        //Quickly setup the simulation data for the 6-wheeled vehicle.
        //(this will copy wheel4 from wheel0 and wheel5 from wheel1).
        driveSimData6W=driveData4W;
        wheelsSimData6W.copy(*wheelsSimData4W,0,0);
        wheelsSimData6W.copy(*wheelsSimData4W,1,1);
        wheelsSimData6W.copy(*wheelsSimData4W,2,2);
        wheelsSimData6W.copy(*wheelsSimData4W,3,3);
        wheelsSimData6W.copy(*wheelsSimData4W,0,4);
        wheelsSimData6W.copy(*wheelsSimData4W,1,5);
        wheelsSimData6W.setTireLoadFilterData(wheelsSimData4W->getTireLoadFilterData());
        chassisData6W = chassisData4W;

        //Make sure that the two non-driven wheels don't respond to the handbrake.
        PxVehicleWheelData wheelData;
        wheelData=wheelsSimData6W.getWheelData(4);
        wheelData.mMaxHandBrakeTorque=0.0f;
        wheelsSimData6W.setWheelData(4,wheelData);
        wheelData=wheelsSimData6W.getWheelData(5);
        wheelData.mMaxHandBrakeTorque=0.0f;
        wheelsSimData6W.setWheelData(5,wheelData);

        //We've now got a 6-wheeled vehicle but the offsets of the 2 extra wheels are still incorrect.
        //Lets set up the 2 extra wheels to lie on an axle that goes through the centre of the car
        //and is parallel to the front and rear axles.
        {
                PxVec3 w;
                PxF32 offsetLeft;
                PxF32 offsetRight;

                offsetLeft=0.5f*(wheelsSimData6W.getWheelCentreOffset(0).z+wheelsSimData6W.getWheelCentreOffset(2).z);
                w=wheelsSimData4W->getWheelCentreOffset(0);
                w.z=offsetLeft;
                wheelsSimData6W.setWheelCentreOffset(4,w);
                offsetRight=0.5f*(wheelsSimData6W.getWheelCentreOffset(1).z+wheelsSimData6W.getWheelCentreOffset(3).z);
                w=wheelsSimData6W.getWheelCentreOffset(1);
                w.z=offsetRight;
                wheelsSimData6W.setWheelCentreOffset(5,w);

                offsetLeft=0.5f*(wheelsSimData6W.getSuspForceAppPointOffset(0).z+wheelsSimData6W.getSuspForceAppPointOffset(2).z);
                w=wheelsSimData4W->getSuspForceAppPointOffset(0);
                w.z=offsetLeft;
                wheelsSimData6W.setSuspForceAppPointOffset(4,w);
                offsetRight=0.5f*(wheelsSimData6W.getSuspForceAppPointOffset(1).z+wheelsSimData6W.getSuspForceAppPointOffset(3).z);
                w=wheelsSimData6W.getSuspForceAppPointOffset(1);
                w.z=offsetRight;
                wheelsSimData6W.setSuspForceAppPointOffset(5,w);

                offsetLeft=0.5f*(wheelsSimData6W.getTireForceAppPointOffset(0).z+wheelsSimData6W.getTireForceAppPointOffset(2).z);
                w=wheelsSimData4W->getTireForceAppPointOffset(0);
                w.z=offsetLeft;
                wheelsSimData6W.setTireForceAppPointOffset(4,w);
                offsetRight=0.5f*(wheelsSimData6W.getTireForceAppPointOffset(1).z+wheelsSimData6W.getTireForceAppPointOffset(3).z);
                w=wheelsSimData6W.getTireForceAppPointOffset(1);
                w.z=offsetRight;
                wheelsSimData6W.setTireForceAppPointOffset(5,w);
        }

        //The first 4 wheels were all set up to support a mass M but now that mass is
        //distributed between 6 wheels.  Adjust the suspension springs accordingly
        //and try to preserve the natural frequency and damping ratio of the springs.
        for(PxU32 i=0;i<4;i++)
        {
                PxVehicleSuspensionData suspData=wheelsSimData6W.getSuspensionData(i);
                suspData.mSprungMass*=0.6666f;
                //suspData.mSpringStrength*=0.666f;
                suspData.mSpringDamperRate*=0.666f;
                suspData.mMaxCompression*=0.666f;
                wheelsSimData6W.setSuspensionData(i,suspData);
        }
        const PxF32 sprungMass=(wheelsSimData6W.getSuspensionData(0).mSprungMass+wheelsSimData6W.getSuspensionData(3).mSprungMass)/2.0f;
        const PxF32 strength=(wheelsSimData6W.getSuspensionData(0).mSpringStrength+wheelsSimData6W.getSuspensionData(3).mSpringStrength)/2.0f;
        const PxF32 damperRate=(wheelsSimData6W.getSuspensionData(0).mSpringDamperRate+wheelsSimData6W.getSuspensionData(3).mSpringDamperRate)/2.0f;
        PxVehicleSuspensionData suspData4=wheelsSimData6W.getSuspensionData(4);
        suspData4.mSprungMass=sprungMass;
        suspData4.mSpringStrength=strength;
        suspData4.mSpringDamperRate=damperRate;
        wheelsSimData6W.setSuspensionData(4,suspData4);
        PxVehicleSuspensionData suspData5=wheelsSimData6W.getSuspensionData(5);
        suspData5.mSprungMass=sprungMass;
        suspData5.mSpringStrength=strength;
        suspData5.mSpringDamperRate=damperRate;
        wheelsSimData6W.setSuspensionData(5,suspData5);

        //Free the 4W sim data because we don't need this any more.
        wheelsSimData4W->free();
}
#endif

void createVehicle6WSimulationData
(const PxF32 chassisMass, const PxVec3* wheelCentreOffsets4, PxConvexMesh* chassisConvexMesh, PxConvexMesh** wheelConvexMeshes4,
 PxVehicleWheelsSimData& wheelsSimData6W, PxVehicleDriveSimDataNW& driveSimData6W, PxVehicleChassisData& chassisData6W)
{
        std::cout << __LINE__ << ": passed" << std::endl;

        //Start by constructing the simulation data for a 4-wheeled vehicle.
        PxVehicleWheelsSimData* wheelsSimData4W=PxVehicleWheelsSimData::allocate(4);
        PxVehicleDriveSimData4W driveData4W;
        PxVehicleChassisData chassisData4W;


        createVehicle4WSimulationData(
                chassisMass,chassisConvexMesh,
                20.0f,wheelConvexMeshes4,wheelCentreOffsets4,
                *wheelsSimData4W,driveData4W,chassisData4W);

        std::cout << __LINE__ << ": passed" << std::endl;

        //Quickly setup the simulation data for the 6-wheeled vehicle.
        //(this will copy wheel4 from wheel0 and wheel5 from wheel1).
        driveSimData6W.setAutoBoxData(driveData4W.getAutoBoxData());
        driveSimData6W.setClutchData(driveData4W.getClutchData());
        driveSimData6W.setEngineData(driveData4W.getEngineData());
        driveSimData6W.setGearsData(driveData4W.getGearsData());
        PxVehicleDifferentialNWData diffNW;
        diffNW.setDrivenWheel(0,true);
        diffNW.setDrivenWheel(1,true);
        diffNW.setDrivenWheel(2,true);
        diffNW.setDrivenWheel(3,true);
        diffNW.setDrivenWheel(4,true);
        diffNW.setDrivenWheel(5,true);
        driveSimData6W.setDiffData(diffNW);
        wheelsSimData6W.copy(*wheelsSimData4W,0,0);
        wheelsSimData6W.copy(*wheelsSimData4W,1,1);
        wheelsSimData6W.copy(*wheelsSimData4W,2,2);
        wheelsSimData6W.copy(*wheelsSimData4W,3,3);
        wheelsSimData6W.copy(*wheelsSimData4W,0,4);
        wheelsSimData6W.copy(*wheelsSimData4W,1,5);
        wheelsSimData6W.setTireLoadFilterData(wheelsSimData4W->getTireLoadFilterData());
        chassisData6W = chassisData4W;

        //Make sure that the two non-driven wheels don't respond to the handbrake.
        PxVehicleWheelData wheelData;
        wheelData=wheelsSimData6W.getWheelData(4);
        wheelData.mMaxHandBrakeTorque=0.0f;
        wheelsSimData6W.setWheelData(4,wheelData);
        wheelData=wheelsSimData6W.getWheelData(5);
        wheelData.mMaxHandBrakeTorque=0.0f;
        wheelsSimData6W.setWheelData(5,wheelData);

        //We've now got a 6-wheeled vehicle but the offsets of the 2 extra wheels are still incorrect.
        //Lets set up the 2 extra wheels to lie on an axle that goes through the centre of the car
        //and is parallel to the front and rear axles.
        {
                PxVec3 w;
                PxF32 offsetLeft;
                PxF32 offsetRight;

#if 0
                offsetLeft=0.5f*(wheelsSimData6W.getWheelCentreOffset(0).z+wheelsSimData6W.getWheelCentreOffset(2).z);
                w=wheelsSimData4W->getWheelCentreOffset(0);
                w.z=offsetLeft;
                wheelsSimData6W.setWheelCentreOffset(4,w);
                offsetRight=0.5f*(wheelsSimData6W.getWheelCentreOffset(1).z+wheelsSimData6W.getWheelCentreOffset(3).z);
                w=wheelsSimData6W.getWheelCentreOffset(1);
                w.z=offsetRight;
                wheelsSimData6W.setWheelCentreOffset(5,w);
#else
                PxF32 offset = 1.7;

                offsetLeft=wheelsSimData6W.getWheelCentreOffset(2).z + offset;
                w=wheelsSimData4W->getWheelCentreOffset(0);
                w.z=offsetLeft;
                wheelsSimData6W.setWheelCentreOffset(4,w);
                offsetRight=wheelsSimData6W.getWheelCentreOffset(3).z + offset;
                w=wheelsSimData6W.getWheelCentreOffset(1);
                w.z=offsetRight;
                wheelsSimData6W.setWheelCentreOffset(5,w);

#endif

                offsetLeft=0.5f*(wheelsSimData6W.getSuspForceAppPointOffset(0).z+wheelsSimData6W.getSuspForceAppPointOffset(2).z);
                w=wheelsSimData4W->getSuspForceAppPointOffset(0);
                w.z=offsetLeft;
                wheelsSimData6W.setSuspForceAppPointOffset(4,w);
                offsetRight=0.5f*(wheelsSimData6W.getSuspForceAppPointOffset(1).z+wheelsSimData6W.getSuspForceAppPointOffset(3).z);
                w=wheelsSimData6W.getSuspForceAppPointOffset(1);
                w.z=offsetRight;
                wheelsSimData6W.setSuspForceAppPointOffset(5,w);

                offsetLeft=0.5f*(wheelsSimData6W.getTireForceAppPointOffset(0).z+wheelsSimData6W.getTireForceAppPointOffset(2).z);
                w=wheelsSimData4W->getTireForceAppPointOffset(0);
                w.z=offsetLeft;
                wheelsSimData6W.setTireForceAppPointOffset(4,w);
                offsetRight=0.5f*(wheelsSimData6W.getTireForceAppPointOffset(1).z+wheelsSimData6W.getTireForceAppPointOffset(3).z);
                w=wheelsSimData6W.getTireForceAppPointOffset(1);
                w.z=offsetRight;
                wheelsSimData6W.setTireForceAppPointOffset(5,w);
        }

        //The first 4 wheels were all set up to support a mass M but now that mass is
        //distributed between 6 wheels.  Adjust the suspension springs accordingly
        //and try to preserve the natural frequency and damping ratio of the springs.
        for(PxU32 i=0;i<4;i++)
        {
                PxVehicleSuspensionData suspData=wheelsSimData6W.getSuspensionData(i);
                suspData.mSprungMass*=0.6666f;
                suspData.mSpringStrength*=0.666f;
                suspData.mSpringDamperRate*=0.666f;
                wheelsSimData6W.setSuspensionData(i,suspData);
        }
        const PxF32 sprungMass=(wheelsSimData6W.getSuspensionData(0).mSprungMass+wheelsSimData6W.getSuspensionData(3).mSprungMass)/2.0f;
        const PxF32 strength=(wheelsSimData6W.getSuspensionData(0).mSpringStrength+wheelsSimData6W.getSuspensionData(3).mSpringStrength)/2.0f;
        const PxF32 damperRate=(wheelsSimData6W.getSuspensionData(0).mSpringDamperRate+wheelsSimData6W.getSuspensionData(3).mSpringDamperRate)/2.0f;
        PxVehicleSuspensionData suspData4=wheelsSimData6W.getSuspensionData(4);
        suspData4.mSprungMass=sprungMass;
        suspData4.mSpringStrength=strength;
        suspData4.mSpringDamperRate=damperRate;
        wheelsSimData6W.setSuspensionData(4,suspData4);
        PxVehicleSuspensionData suspData5=wheelsSimData6W.getSuspensionData(5);
        suspData5.mSprungMass=sprungMass;
        suspData5.mSpringStrength=strength;
        suspData5.mSpringDamperRate=damperRate;
        wheelsSimData6W.setSuspensionData(5,suspData5);

        //Free the 4W sim data because we don't need this any more.
        wheelsSimData4W->free();
}

PxF32 gChassisMass=2500.0f;
PxF32 gSuspensionShimHeight=0.125f;


PxVehicleDriveNW* create6WVehicle(PxScene& scene, PxPhysics& physics, const PxMaterial& material,
 const PxF32 chassisMass, const PxVec3* wheelCentreOffsets4, PxConvexMesh* chassisConvexMesh, PxConvexMesh** wheelConvexMeshes4)
{
        std::cout << __LINE__ << ": passed" << std::endl;
        //We're going to create an 4-wheeled vehicle then quickly copy components to make an 6-wheeled vehicle.
        PxVehicleWheelsSimData* wheelsSimData=PxVehicleWheelsSimData::allocate(6);
        PxVehicleDriveSimDataNW driveSimData;
        PxVehicleChassisData chassisData;

        std::cout << __LINE__ << ": passed" << std::endl;

        createVehicle6WSimulationData(chassisMass,wheelCentreOffsets4,chassisConvexMesh,wheelConvexMeshes4,*wheelsSimData,driveSimData,chassisData);

        std::cout << __LINE__ << ": passed" << std::endl;

        //Instantiate and finalize the vehicle using physx.
        PxRigidDynamic* vehActor=createVehicleActor6W(chassisData,wheelConvexMeshes4,chassisConvexMesh,scene,physics,material);

        //Create a car.
        PxVehicleDriveNW* car = PxVehicleDriveNW::allocate(6);
        car->setup(&physics,vehActor,*wheelsSimData,driveSimData,6);

        //Free the sim data because we don't need that any more.
        wheelsSimData->free();

        //Don't forget to add the actor to the scene.
        {
                //PxSceneWriteLock scopedLock(scene);
                //scene.addActor(*vehActor);
        }

        //Set up the mapping between wheel and actor shape.
        car->mWheelsSimData.setWheelShapeMapping(0,0);
        car->mWheelsSimData.setWheelShapeMapping(1,1);
        car->mWheelsSimData.setWheelShapeMapping(2,2);
        car->mWheelsSimData.setWheelShapeMapping(3,3);
        car->mWheelsSimData.setWheelShapeMapping(4,4);
        car->mWheelsSimData.setWheelShapeMapping(5,5);

        //Set up the scene query filter data for each suspension line.
        PxFilterData vehQryFilterData;
        SampleVehicleSetupVehicleShapeQueryFilterData(&vehQryFilterData);
        car->mWheelsSimData.setSceneQueryFilterData(0, vehQryFilterData);
        car->mWheelsSimData.setSceneQueryFilterData(1, vehQryFilterData);
        car->mWheelsSimData.setSceneQueryFilterData(2, vehQryFilterData);
        car->mWheelsSimData.setSceneQueryFilterData(3, vehQryFilterData);
        car->mWheelsSimData.setSceneQueryFilterData(4, vehQryFilterData);
        car->mWheelsSimData.setSceneQueryFilterData(5, vehQryFilterData);

        //Set the transform and the instantiated car and set it be to be at rest.
        //resetNWCar(startTransform,car);
        //Set the autogear mode of the instantiate car.
        car->mDriveDynData.setUseAutoGears(true);

        //Increment the number of vehicles
#if 0
        mVehicles[mNumVehicles]=car;
        mVehicleWheelQueryResults[mNumVehicles].nbWheelQueryResults=6;
        mVehicleWheelQueryResults[mNumVehicles].wheelQueryResults=mWheelQueryResults->addVehicle(6);
        mNumVehicles++;
#endif
        return car;
}

//The class WheelContactModifyCallback identifies and modifies rigid body contacts
//that involve a wheel.  Contacts that can be identified and managed by the suspension
//system are ignored.  Any contacts that remain are modified to account for the rotation
//speed of the wheel around the rolling axis.
class WheelContactModifyCallback : public PxContactModifyCallback
{
public:

	WheelContactModifyCallback()
		: PxContactModifyCallback()
	{
	}

	~WheelContactModifyCallback(){}

	void onContactModify(PxContactModifyPair* const pairs, PxU32 count) 
	{
		for(PxU32 i = 0; i < count ; i++)
		{
			const PxRigidActor** actors = pairs[i].actor;
			const PxShape** shapes = pairs[i].shape;

			//Search for actors that represent vehicles and shapes that represent wheels.
			for(PxU32 j = 0; j < 2; j++)
			{
				const PxActor* actor = actors[j];
				if(actor->userData && (static_cast<ActorUserData*>(actor->userData))->vehicle)
				{
					const PxVehicleWheels* vehicle = (static_cast<ActorUserData*>(actor->userData))->vehicle;
					PX_ASSERT(vehicle->getRigidDynamicActor() == actors[j]);

					const PxShape* shape = shapes[j];
					if(shape->userData && (static_cast<ShapeUserData*>(shape->userData))->isWheel)
					{
						const PxU32 wheelId = (static_cast<ShapeUserData*>(shape->userData))->wheelId;
						PX_ASSERT(wheelId < vehicle->mWheelsSimData.getNbWheels());

						//Modify wheel contacts.
						PxVehicleModifyWheelContacts(*vehicle, wheelId, WHEEL_TANGENT_VELOCITY_MULTIPLIER, MAX_IMPULSE, pairs[i]);
					}
				}
			}
		}
	}

private:

};
WheelContactModifyCallback gWheelContactModifyCallback;

//The class WheelCCDContactModifyCallback identifies and modifies ccd contacts
//that involve a wheel.  Contacts that can be identified and managed by the suspension
//system are ignored.  Any contacts that remain are modified to account for the rotation
//speed of the wheel around the rolling axis.
class WheelCCDContactModifyCallback : public PxCCDContactModifyCallback
{
public:

	WheelCCDContactModifyCallback()
		: PxCCDContactModifyCallback()
	{
	}

	~WheelCCDContactModifyCallback(){}

	void onCCDContactModify(PxContactModifyPair* const pairs, PxU32 count) 
	{
		for(PxU32 i = 0; i < count ; i++)
		{
			const PxRigidActor** actors = pairs[i].actor;
			const PxShape** shapes = pairs[i].shape;

			//Search for actors that represent vehicles and shapes that represent wheels.
			for(PxU32 j = 0; j < 2; j++)
			{
				const PxActor* actor = actors[j];
				if(actor->userData && (static_cast<ActorUserData*>(actor->userData))->vehicle)
				{
					const PxVehicleWheels* vehicle = (static_cast<ActorUserData*>(actor->userData))->vehicle;
					PX_ASSERT(vehicle->getRigidDynamicActor() == actors[j]);

					const PxShape* shape = shapes[j];
					if(shape->userData && (static_cast<ShapeUserData*>(shape->userData))->isWheel)
					{
						const PxU32 wheelId = (static_cast<ShapeUserData*>(shape->userData))->wheelId;
						PX_ASSERT(wheelId < vehicle->mWheelsSimData.getNbWheels());

						//Modify wheel contacts.
						PxVehicleModifyWheelContacts(*vehicle, wheelId, WHEEL_TANGENT_VELOCITY_MULTIPLIER, MAX_IMPULSE, pairs[i]);
					}
				}
			}
		}
	}
};
WheelCCDContactModifyCallback gWheelCCDContactModifyCallback;

VehicleDesc initCabDesc(const PxFilterData& chassisSimFilterData, const PxU32 vehicleId)
{
    const PxF32 chassisMass = 50.0f;
    PxVec3 chassisDims(2.5f,2.5f,2.0f);


    const PxVec3 chassisMOI
            ((chassisDims.y*chassisDims.y + chassisDims.z*chassisDims.z)*chassisMass/12.0f,
             (chassisDims.x*chassisDims.x + chassisDims.z*chassisDims.z)*0.8f*chassisMass/12.0f,
             (chassisDims.x*chassisDims.x + chassisDims.y*chassisDims.y)*chassisMass/12.0f);

    PxVec3 chassisCMOffset(0.0f, -chassisDims.y*0.5f + 0.65f, 0.25f);

    VehicleDesc vehicleDesc;

    vehicleDesc.chassisMass = chassisMass;
    vehicleDesc.chassisDims = chassisDims;
    vehicleDesc.chassisMOI = chassisMOI;
    vehicleDesc.chassisCMOffset = chassisCMOffset;
    vehicleDesc.chassisMaterial = gMaterial;
    vehicleDesc.chassisSimFilterData = chassisSimFilterData;

    vehicleDesc.actorUserData = &gCabinActorUserData[vehicleId];
    vehicleDesc.shapeUserDatas = gCabinShapeUserDatas[vehicleId];

    return vehicleDesc;
}

VehicleDesc initVehicleDesc(const PxFilterData& chassisSimFilterData, const PxFilterData& wheelSimFilterData, const PxU32 vehicleId, bool trailer = false)
{
	//Set up the chassis mass, dimensions, moment of inertia, and center of mass offset.
	//The moment of inertia is just the moment of inertia of a cuboid but modified for easier steering.
	//Center of mass offset is 0.65m above the base of the chassis and 0.25m towards the front.
        const PxF32 chassisMass = trailer ? 1000.0f : 3500.0f;
        PxVec3 chassisDims(2.5f,0.5f,5.0f);

        chassisDims.z *= trailer ? 1.5f : 1.f;        

        const PxVec3 chassisMOI
		((chassisDims.y*chassisDims.y + chassisDims.z*chassisDims.z)*chassisMass/12.0f,
		 (chassisDims.x*chassisDims.x + chassisDims.z*chassisDims.z)*0.8f*chassisMass/12.0f,
		 (chassisDims.x*chassisDims.x + chassisDims.y*chassisDims.y)*chassisMass/12.0f);

        PxVec3 chassisCMOffset(0.0f, -chassisDims.y*0.5f + 0.65f, 0.25f);        

	//Set up the wheel mass, radius, width, moment of inertia, and number of wheels.
	//Moment of inertia is just the moment of inertia of a cylinder.
	const PxF32 wheelMass = 20.0f;
	const PxF32 wheelRadius = 0.5f;
	const PxF32 wheelWidth = 0.4f;
	const PxF32 wheelMOI = 0.5f*wheelMass*wheelRadius*wheelRadius;
        const PxU32 nbWheels = 6;

	VehicleDesc vehicleDesc;

	vehicleDesc.chassisMass = chassisMass;
	vehicleDesc.chassisDims = chassisDims;
	vehicleDesc.chassisMOI = chassisMOI;
	vehicleDesc.chassisCMOffset = chassisCMOffset;
	vehicleDesc.chassisMaterial = gMaterial;
	vehicleDesc.chassisSimFilterData = chassisSimFilterData;

	vehicleDesc.wheelMass = wheelMass;
	vehicleDesc.wheelRadius = wheelRadius;
	vehicleDesc.wheelWidth = wheelWidth;
	vehicleDesc.wheelMOI = wheelMOI;
	vehicleDesc.numWheels = nbWheels;
	vehicleDesc.wheelMaterial = gMaterial;
	vehicleDesc.wheelSimFilterData = wheelSimFilterData;

        if (trailer)
        {
            vehicleDesc.actorUserData = &gTrailerActorUserData[vehicleId];
            vehicleDesc.shapeUserDatas = gTrailerShapeUserDatas[vehicleId];
        }
        else
        {
            vehicleDesc.actorUserData = &gActorUserData[vehicleId];
            vehicleDesc.shapeUserDatas = gShapeUserDatas[vehicleId];
        }

        vehicleDesc.trailer = trailer;

	return vehicleDesc;
}

void initPhysics()
{
	gFoundation = PxCreateFoundation(PX_PHYSICS_VERSION, gAllocator, gErrorCallback);
	gPvd = PxCreatePvd(*gFoundation);
	PxPvdTransport* transport = PxDefaultPvdSocketTransportCreate(PVD_HOST, 5425, 10);
	gPvd->connect(*transport,PxPvdInstrumentationFlag::eALL);

	gPhysics = PxCreatePhysics(PX_PHYSICS_VERSION, *gFoundation, PxTolerancesScale(),true, gPvd);

	PxSceneDesc sceneDesc(gPhysics->getTolerancesScale());
	sceneDesc.gravity = PxVec3(0.0f, -9.81f, 0.0f);
	
	PxU32 numWorkers = 1;
	gDispatcher = PxDefaultCpuDispatcherCreate(numWorkers);
	sceneDesc.cpuDispatcher	= gDispatcher;
	sceneDesc.filterShader	= VehicleFilterShader;							//Set the filter shader
        sceneDesc.contactModifyCallback = &gWheelContactModifyCallback;			//Enable contact modification
        sceneDesc.ccdContactModifyCallback = &gWheelCCDContactModifyCallback;	//Enable ccd contact modification
        sceneDesc.flags |= PxSceneFlag::eENABLE_CCD;							//Enable ccd

	gScene = gPhysics->createScene(sceneDesc);
	PxPvdSceneClient* pvdClient = gScene->getScenePvdClient();
	if(pvdClient)
	{
		pvdClient->setScenePvdFlag(PxPvdSceneFlag::eTRANSMIT_CONSTRAINTS, true);
		pvdClient->setScenePvdFlag(PxPvdSceneFlag::eTRANSMIT_CONTACTS, true);
		pvdClient->setScenePvdFlag(PxPvdSceneFlag::eTRANSMIT_SCENEQUERIES, true);
	}
	gMaterial = gPhysics->createMaterial(0.1f, 0.1f, 0.01f);

	gCooking = 	PxCreateCooking(PX_PHYSICS_VERSION, *gFoundation, PxCookingParams(PxTolerancesScale()));	

	/////////////////////////////////////////////

	PxInitVehicleSDK(*gPhysics);
	PxVehicleSetBasisVectors(PxVec3(0,1,0), PxVec3(0,0,1));
	PxVehicleSetUpdateMode(PxVehicleUpdateMode::eVELOCITY_CHANGE);
	PxVehicleSetSweepHitRejectionAngles(POINT_REJECT_ANGLE, NORMAL_REJECT_ANGLE);
	PxVehicleSetMaxHitActorAcceleration(MAX_ACCELERATION);

	//Create the batched scene queries for the suspension sweeps.
	//Use the post-filter shader to reject hit shapes that overlap the swept wheel at the start pose of the sweep.
	PxQueryHitType::Enum(*sceneQueryPreFilter)(PxFilterData, PxFilterData, const void*, PxU32, PxHitFlags&);
	PxQueryHitType::Enum(*sceneQueryPostFilter)(PxFilterData, PxFilterData, const void*, PxU32, const PxQueryHit&);
#if BLOCKING_SWEEPS
	sceneQueryPreFilter = &WheelSceneQueryPreFilterBlocking;
	sceneQueryPostFilter = &WheelSceneQueryPostFilterBlocking;
#else
	sceneQueryPreFilter = &WheelSceneQueryPreFilterNonBlocking;
	sceneQueryPostFilter = &WheelSceneQueryPostFilterNonBlocking;
#endif 
	gVehicleSceneQueryData = VehicleSceneQueryData::allocate(NUM_VEHICLES, PX_MAX_NB_WHEELS, gNbQueryHitsPerWheel, NUM_VEHICLES, sceneQueryPreFilter, sceneQueryPostFilter, gAllocator);
	gBatchQuery = VehicleSceneQueryData::setUpBatchedSceneQuery(0, *gVehicleSceneQueryData, gScene);

	//Create the friction table for each combination of tire and surface type.
	gFrictionPairs = createFrictionPairs(gMaterial);
	
	//Create a plane to drive on.
	PxFilterData groundPlaneSimFilterData(COLLISION_FLAG_GROUND, COLLISION_FLAG_GROUND_AGAINST, 0, 0);
	gGroundPlane = createDrivablePlane(groundPlaneSimFilterData, gMaterial, gPhysics);
	gScene->addActor(*gGroundPlane);

	//Create several static obstacles for the first vehicle to drive on.
	//  (i) collide only with wheel shapes
	// (ii) have continuous collision detection (CCD) enabled
	//(iii) have contact modification enabled
	// (iv) are configured to be drivable surfaces
        const PxF32 capsuleRadii[12] = {0.05f, 	0.05f, 0.05f, 0.05f, 0.05f, 	0.05f, 0.05f, 0.05f, 0.05f, 	0.05f, 0.05f, 0.05f};
        const PxF32 capsuleZ[12] = {5.0f, 10.0f, 15.0f, 20.0f, 25.0f, 30.0f, 35.0f, 40.0f, 45.0f, 50.0f, 55.0f, 60.0f};
        for(PxU32 i = 0; i < 12; i++)
	{
		PxTransform t(PxVec3(xCoordVehicleStarts[0], capsuleRadii[i], capsuleZ[i]), PxQuat(PxIdentity));
		PxRigidStatic* rd = gPhysics->createRigidStatic(t);
		PxCapsuleGeometry capsuleGeom(capsuleRadii[i], 3.0f);
		PxShape* shape = PxRigidActorExt::createExclusiveShape(*rd, capsuleGeom, *gMaterial);
                PxFilterData simFilterData(COLLISION_FLAG_OBSTACLE, COLLISION_FLAG_WHEEL, PxPairFlag::eMODIFY_CONTACTS | PxPairFlag::eDETECT_CCD_CONTACT, 0);
		shape->setSimulationFilterData(simFilterData);
		PxFilterData qryFilterData;
		setupDrivableSurface(qryFilterData);
		shape->setQueryFilterData(qryFilterData);
		gScene->addActor(*rd);
	}
	const PxF32 boxHalfHeights[1] = {1.0f};
        const PxF32 boxZ[1] = {80.0f};
	for(PxU32 i = 0; i < 1; i++)
	{
		PxTransform t(PxVec3(xCoordVehicleStarts[0], boxHalfHeights[i], boxZ[i]), PxQuat(PxIdentity));
		PxRigidStatic* rd = gPhysics->createRigidStatic(t);

		PxBoxGeometry boxGeom(PxVec3(3.0f, boxHalfHeights[i], 3.0f));
		PxShape* shape = PxRigidActorExt::createExclusiveShape(*rd, boxGeom, *gMaterial);
		
                PxFilterData simFilterData(COLLISION_FLAG_OBSTACLE, COLLISION_FLAG_WHEEL, PxPairFlag::eMODIFY_CONTACTS | PxPairFlag::eDETECT_CCD_CONTACT, 0);
		shape->setSimulationFilterData(simFilterData);
		PxFilterData qryFilterData;
		setupDrivableSurface(qryFilterData);
		shape->setQueryFilterData(qryFilterData);
		
		gScene->addActor(*rd);
	}

	//Create a pile of dynamic objects for the second vehicle to drive on.
	//  (i) collide only with wheel shapes
	// (ii) have continuous collision detection (CCD) enabled
	//(iii) have contact modification enabled
	// (iv) are configured to be drivable surfaces
	{
		for (PxU32 i = 0; i < 64; i++)
		{
			PxTransform t(PxVec3(xCoordVehicleStarts[1] + i*0.01f, 2.0f + i*0.25f, 20.0f + i*0.025f), PxQuat(PxPi*0.5f, PxVec3(0, 1, 0)));
			PxRigidDynamic* rd = gPhysics->createRigidDynamic(t);
	
			PxBoxGeometry boxGeom(PxVec3(0.08f, 0.25f, 1.0f));
			PxShape* shape = PxRigidActorExt::createExclusiveShape(*rd, boxGeom, *gMaterial);

			PxFilterData simFilterData(COLLISION_FLAG_OBSTACLE, COLLISION_FLAG_OBSTACLE_AGAINST, PxPairFlag::eMODIFY_CONTACTS | PxPairFlag::eDETECT_CCD_CONTACT, 0);
			shape->setSimulationFilterData(simFilterData);
			PxFilterData qryFilterData;
			setupDrivableSurface(qryFilterData);
			shape->setQueryFilterData(qryFilterData);

			PxRigidBodyExt::updateMassAndInertia(*rd, 30.0f);

			gScene->addActor(*rd);
		}
	}

        std::cout << __LINE__ << ": passed" << std::endl;

	//Create two vehicles that will drive on the obstacles.
	//The vehicles are configured with wheels that 
	//  (i) collide with obstacles
	// (ii) have continuous collision detection (CCD) enabled
	//(iii) have contact modification enabled
	//The vehicle chassis only collides with the ground to highlight the collision between the wheels and the obstacles.
	for (PxU32 i = 0; i < NUM_VEHICLES; i++)
	{
                PxFilterData chassisSimFilterData(COLLISION_FLAG_CHASSIS, COLLISION_FLAG_GROUND, 0, 0);
                PxFilterData wheelSimFilterData(COLLISION_FLAG_WHEEL, COLLISION_FLAG_WHEEL, PxPairFlag::eDETECT_CCD_CONTACT | PxPairFlag::eMODIFY_CONTACTS, 0);
                std::cout << __LINE__ << ": passed" << std::endl;
		VehicleDesc vehicleDesc = initVehicleDesc(chassisSimFilterData, wheelSimFilterData, i);
                std::cout << __LINE__ << ": passed" << std::endl;

                gChassisConvexMesh[i] = createChassisMesh(PxVec3(2.5,1,6),*gPhysics, *gCooking);

                for (size_t j = 0; j < 4; ++j)
                {
                    gWheelConvexMeshes4[j] = createWheelMesh(0.5,0.75,*gPhysics,*gCooking);
                }

                gWheelCentreOffsets4[0] = PxVec3(-1.25,-1,2.5);
                gWheelCentreOffsets4[1] = PxVec3(1.25,-1,2.5);
                gWheelCentreOffsets4[2] = PxVec3(-1.25,-1,-2.5);
                gWheelCentreOffsets4[3] = PxVec3(1.25,-1,-2.5);

#if 1
                gVehicle4W[i] = createVehicle4W(vehicleDesc, gPhysics, gCooking);
#else
                gVehicle4W[i] = create6WVehicle(*gScene, *gPhysics, *gMaterial, gChassisMass,gWheelCentreOffsets4,gChassisConvexMesh[i],gWheelConvexMeshes4);
#endif

                gScene->addActor(*gVehicle4W[i]->getRigidDynamicActor());


                std::cout << __LINE__ << ": passed" << std::endl;
		PxTransform startTransform(PxVec3(xCoordVehicleStarts[i], (vehicleDesc.chassisDims.y*0.5f + vehicleDesc.wheelRadius + 1.0f), 0), PxQuat(PxIdentity));
                gVehicle4W[i]->getRigidDynamicActor()->setGlobalPose(startTransform, false);
                gVehicle4W[i]->getRigidDynamicActor()->setRigidBodyFlag(PxRigidBodyFlag::eENABLE_CCD, true);



		//Set the vehicle to rest in first gear.
		//Set the vehicle to use auto-gears.
		gVehicle4W[i]->setToRestState();
                gVehicle4W[i]->mDriveDynData.forceGearChange(PxVehicleGearsData::eFIRST);
		gVehicle4W[i]->mDriveDynData.setUseAutoGears(true);



                chassisSimFilterData = PxFilterData(0, COLLISION_FLAG_GROUND, 0, 0);
                vehicleDesc = initVehicleDesc(chassisSimFilterData, wheelSimFilterData, i, true);

                gTrailer[i] = createVehicle4W(vehicleDesc, gPhysics, gCooking);

                gScene->addActor(*gTrailer[i]->getRigidDynamicActor());


                //Set the vehicle to rest in first gear.
                //Set the vehicle to use auto-gears.
                startTransform.p.z += 7.f;
                gTrailer[i]->getRigidDynamicActor()->setGlobalPose(startTransform,false);
                gTrailer[i]->mDriveDynData.forceGearChange(PxVehicleGearsData::eFIRST);
                gTrailer[i]->mDriveDynData.setUseAutoGears(true);
                gTrailer[i]->getRigidDynamicActor()->setRigidBodyFlag(PxRigidBodyFlag::eENABLE_CCD, false);


                //Set the vehicle to rest in first gear.
                //Set the vehicle to use auto-gears.
                gTrailer[i]->setToRestState();

                PxTransform jointPoint(PxVec3(0.f, 0.f, -2.f), PxQuat(PxIdentity));
                PxTransform jointPointTrailer(PxVec3(0.f, 0.f, 3.25f), PxQuat(PxIdentity));

                PxD6Joint* j = PxD6JointCreate(*gPhysics, gVehicle4W[i]->getRigidDynamicActor(), jointPoint, gTrailer[i]->getRigidDynamicActor(), jointPointTrailer);
                j->setMotion(PxD6Axis::eSWING1, PxD6Motion::eFREE);
                j->setMotion(PxD6Axis::eSWING2, PxD6Motion::eFREE);
                j->setDrive(PxD6Drive::eSLERP, PxD6JointDrive(0, 100, FLT_MAX, true));

                VehicleDesc cabDesc = initCabDesc(chassisSimFilterData, i);
                gCab[i] = createCabin(cabDesc, gVehicle4W[i]->getRigidDynamicActor(),gPhysics, gCooking);




                gScene->addActor(*gCab[i]);
	}
}

void stepPhysics()
{
	const PxF32 timestep = 1.0f/60.0f;

	//Set the vehicles to accelerate forwards.
	for (PxU32 i = 0; i < 2; i++)
	{
                gVehicle4W[i]->mDriveDynData.setAnalogInput(PxVehicleDrive4WControl::eANALOG_INPUT_ACCEL, 0.25f);
                //gVehicle4W[i]->mDriveDynData.setAnalogInput(PxVehicleDrive4WControl::eANALOG_INPUT_STEER_LEFT, 0.25f);
	}

	//Scene update.
	gScene->simulate(timestep);
	gScene->fetchResults(true);

	//Suspension sweeps (instead of raycasts).
	//Sweeps provide more information about the geometry under the wheel.
        PxVehicleWheels* vehicles[NUM_VEHICLES*2] = {gVehicle4W[0], gVehicle4W[1], gTrailer[0], gTrailer[1]};
	PxSweepQueryResult* sweepResults = gVehicleSceneQueryData->getSweepQueryResultBuffer(0);
	const PxU32 sweepResultsSize = gVehicleSceneQueryData->getQueryResultBufferSize();
        PxVehicleSuspensionSweeps(gBatchQuery, NUM_VEHICLES*2, vehicles, sweepResultsSize, sweepResults, gNbQueryHitsPerWheel, NULL, 1.0f, 1.01f);

	//Vehicle update.
	const PxVec3 grav = gScene->getGravity();
        PxWheelQueryResult wheelQueryResults[PX_MAX_NB_WHEELS][NUM_VEHICLES*2];
        PxVehicleWheelQueryResult vehicleQueryResults[NUM_VEHICLES*2] =
	{
		{ wheelQueryResults[0], gVehicle4W[0]->mWheelsSimData.getNbWheels() },
		{ wheelQueryResults[1] , gVehicle4W[1]->mWheelsSimData.getNbWheels() },
                { wheelQueryResults[2], gTrailer[0]->mWheelsSimData.getNbWheels() },
                { wheelQueryResults[3] , gTrailer[1]->mWheelsSimData.getNbWheels() },
	};
        PxVehicleUpdates(timestep, grav, *gFrictionPairs, NUM_VEHICLES*2, vehicles, vehicleQueryResults);
}
	
void cleanupPhysics()
{
	for (PxU32 i = 0; i < NUM_VEHICLES; i++)
	{
		gVehicle4W[i]->getRigidDynamicActor()->release();
		gVehicle4W[i]->free();
	}
        for (PxU32 i = 0; i < NUM_VEHICLES; i++)
        {
                gTrailer[i]->getRigidDynamicActor()->release();
                gTrailer[i]->free();
        }
	gGroundPlane->release();
	gBatchQuery->release();
	gVehicleSceneQueryData->free(gAllocator);
	gFrictionPairs->release();
	PxCloseVehicleSDK();

	gMaterial->release();
	gCooking->release();
	gScene->release();
	gDispatcher->release();
	gPhysics->release();
	PxPvdTransport* transport = gPvd->getTransport();
	gPvd->release();
	transport->release();
	
 	gFoundation->release();

	printf("SnippetVehicleContactMod done.\n");
}

void keyPress(unsigned char key, const PxTransform& camera)
{
	PX_UNUSED(camera);
	PX_UNUSED(key);

        for (PxU32 i = 0; i < NUM_VEHICLES; i++)
        {
            gVehicle4W[i]->setToRestState();
            gTrailer[i]->setToRestState();

            PxTransform startTransform(PxVec3(xCoordVehicleStarts[i], (0.5f*0.5f + 0.75f+ 1.0f), 0), PxQuat(PxIdentity));
            gVehicle4W[i]->getRigidDynamicActor()->setGlobalPose(startTransform);

            startTransform.p.z += 7.f;
            gTrailer[i]->getRigidDynamicActor()->setGlobalPose(startTransform);

            gVehicle4W[i]->setToRestState();
            gTrailer[i]->setToRestState();

            gVehicle4W[i]->mDriveDynData.forceGearChange(PxVehicleGearsData::eFIRST);
            gVehicle4W[i]->mDriveDynData.setUseAutoGears(true);

            gTrailer[i]->mDriveDynData.forceGearChange(PxVehicleGearsData::eFIRST);
            gTrailer[i]->mDriveDynData.setUseAutoGears(true);            

            gCab[i]->setGlobalPose(startTransform);
        }
}

int snippetMain(int, const char*const*)
{
#ifdef RENDER_SNIPPET
	extern void renderLoop();
	renderLoop();
#else
	initPhysics();
	PxU32 count = 0;
	PxU32 maxCount =1000;
	while(count < maxCount)
	{
		stepPhysics();
		count++;
	}
	cleanupPhysics();
#endif

	return 0;
}