tovjemam/karo
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
karo/karo.cpp
det-fys d4df7fbc40 stuff
2025-07-29 13:34:29 +02:00

1525 lines
51 KiB
C++
Raw Blame History

// karo.cpp : This file contains the 'main' function. Program execution begins and ends there.
//
//#define GLEW_STATIC
#include <GL/glew.h>
#include <GLFW/glfw3.h>
#include <iostream>
#include <vector>
#include <glm/glm.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <glm/gtc/type_ptr.hpp>
#include <PxPhysics.h>
//#include <physx/PxFoundation.h>
#include <PxConfig.h>
#include <PxPhysicsAPI.h>
#include <vehicle/PxVehicleSDK.h>
//#include <physx/PxFiltering.h>
#include <imgui.h>
#include <imgui_impl_glfw.h>
#include <imgui_impl_opengl3.h>
#include "camera.hpp"
using namespace physx;
struct Vertex {
glm::vec3 pos;
glm::vec3 color;
};
static const char* s_vs_src = R"GLSL(
#version 330 core
layout (location = 0) in vec3 a_pos;
layout (location = 1) in vec3 a_color;
out vec3 u_color;
uniform mat4 u_mvp;
void main() {
gl_Position = u_mvp * vec4(a_pos, 1.0);
u_color = a_color;
}
)GLSL";
static const char* s_fs_src = R"GLSL(
#version 330 core
in vec3 u_color;
out vec4 o_color;
void main() {
o_color = vec4(u_color, 1.0);
}
)GLSL";
static void CheckShaderCompileErrors(GLuint shader) {
GLint success;
GLchar infoLog[512];
glGetShaderiv(shader, GL_COMPILE_STATUS, &success);
if (!success) {
glGetShaderInfoLog(shader, 512, NULL, infoLog);
std::cerr << "Shader compilation failed: " << infoLog << std::endl;
exit(1);
}
}
static void CheckProgramLinkErrors(GLuint program) {
GLint success;
GLchar infoLog[512];
glGetProgramiv(program, GL_LINK_STATUS, &success);
if (!success) {
glGetProgramInfoLog(program, 512, NULL, infoLog);
std::cerr << "Program link failed: " << infoLog << std::endl;
exit(1);
}
}
enum ContactGroup {
CG_NONE = 0,
CG_BOX = (1 << 1),
CG_FLOOR = (1 << 2),
};
//PxFilterFlags FilterShaderExample(
// PxFilterObjectAttributes attributes0, PxFilterData filterData0,
// PxFilterObjectAttributes attributes1, PxFilterData filterData1,
// PxPairFlags& pairFlags, const void* constantBlock, PxU32 constantBlockSize)
//{
// // let triggers through
// if (PxFilterObjectIsTrigger(attributes0) || PxFilterObjectIsTrigger(attributes1))
// {
// pairFlags = PxPairFlag::eTRIGGER_DEFAULT;
// return PxFilterFlag::eDEFAULT;
// }
// // generate contacts for all that were not filtered above
// pairFlags = PxPairFlag::eCONTACT_DEFAULT;
//
// // trigger the contact callback for pairs (A,B) where
// // the filtermask of A contains the ID of B and vice versa.
// if ((filterData0.word0 & filterData1.word1) && (filterData1.word0 & filterData0.word1)) {
// pairFlags |= PxPairFlag::eNOTIFY_TOUCH_FOUND;
// return PxFilterFlag::eDEFAULT;
//
// }
//
// return PxFilterFlag::eSUPPRESS;
//}
//void SetupFiltering(PxShape* shape, PxU32 filterGroup, PxU32 filterMask, PxU32 w3)
//{
// PxFilterData filterData;
// filterData.word0 = filterGroup; // word0 = own ID
// filterData.word1 = filterMask; // word1 = ID mask to filter pairs that trigger a contact callback
// filterData.word3 = w3;
// shape->setSimulationFilterData(filterData);
//}
//Data structure for quick setup of scene queries for suspension queries.
class VehicleSceneQueryData
{
public:
VehicleSceneQueryData();
~VehicleSceneQueryData();
//Allocate scene query data for up to maxNumVehicles and up to maxNumWheelsPerVehicle with numVehiclesInBatch per batch query.
static VehicleSceneQueryData* allocate
(const PxU32 maxNumVehicles, const PxU32 maxNumWheelsPerVehicle, const PxU32 maxNumHitPointsPerWheel, const PxU32 numVehiclesInBatch,
PxBatchQueryPreFilterShader preFilterShader, PxBatchQueryPostFilterShader postFilterShader,
PxAllocatorCallback& allocator);
//Free allocated buffers.
void free(PxAllocatorCallback& allocator);
//Create a PxBatchQuery instance that will be used for a single specified batch.
static PxBatchQuery* setUpBatchedSceneQuery(const PxU32 batchId, const VehicleSceneQueryData& vehicleSceneQueryData, PxScene* scene);
//Return an array of scene query results for a single specified batch.
PxRaycastQueryResult* getRaycastQueryResultBuffer(const PxU32 batchId);
//Return an array of scene query results for a single specified batch.
PxSweepQueryResult* getSweepQueryResultBuffer(const PxU32 batchId);
//Get the number of scene query results that have been allocated for a single batch.
PxU32 getQueryResultBufferSize() const;
private:
//Number of queries per batch
PxU32 mNumQueriesPerBatch;
//Number of hit results per query
PxU32 mNumHitResultsPerQuery;
//One result for each wheel.
PxRaycastQueryResult* mRaycastResults;
PxSweepQueryResult* mSweepResults;
//One hit for each wheel.
PxRaycastHit* mRaycastHitBuffer;
PxSweepHit* mSweepHitBuffer;
//Filter shader used to filter drivable and non-drivable surfaces
PxBatchQueryPreFilterShader mPreFilterShader;
//Filter shader used to reject hit shapes that initially overlap sweeps.
PxBatchQueryPostFilterShader mPostFilterShader;
};
VehicleSceneQueryData::VehicleSceneQueryData()
: mNumQueriesPerBatch(0),
mNumHitResultsPerQuery(0),
mRaycastResults(NULL),
mRaycastHitBuffer(NULL),
mPreFilterShader(NULL),
mPostFilterShader(NULL)
{
}
VehicleSceneQueryData::~VehicleSceneQueryData()
{
}
VehicleSceneQueryData* VehicleSceneQueryData::allocate
(const PxU32 maxNumVehicles, const PxU32 maxNumWheelsPerVehicle, const PxU32 maxNumHitPointsPerWheel, const PxU32 numVehiclesInBatch,
PxBatchQueryPreFilterShader preFilterShader, PxBatchQueryPostFilterShader postFilterShader,
PxAllocatorCallback& allocator)
{
const PxU32 sqDataSize = ((sizeof(VehicleSceneQueryData) + 15) & ~15);
const PxU32 maxNumWheels = maxNumVehicles * maxNumWheelsPerVehicle;
const PxU32 raycastResultSize = ((sizeof(PxRaycastQueryResult) * maxNumWheels + 15) & ~15);
const PxU32 sweepResultSize = ((sizeof(PxSweepQueryResult) * maxNumWheels + 15) & ~15);
const PxU32 maxNumHitPoints = maxNumWheels * maxNumHitPointsPerWheel;
const PxU32 raycastHitSize = ((sizeof(PxRaycastHit) * maxNumHitPoints + 15) & ~15);
const PxU32 sweepHitSize = ((sizeof(PxSweepHit) * maxNumHitPoints + 15) & ~15);
const PxU32 size = sqDataSize + raycastResultSize + raycastHitSize + sweepResultSize + sweepHitSize;
PxU8* buffer = static_cast<PxU8*>(allocator.allocate(size, NULL, NULL, 0));
VehicleSceneQueryData* sqData = new(buffer) VehicleSceneQueryData();
sqData->mNumQueriesPerBatch = numVehiclesInBatch * maxNumWheelsPerVehicle;
sqData->mNumHitResultsPerQuery = maxNumHitPointsPerWheel;
buffer += sqDataSize;
sqData->mRaycastResults = reinterpret_cast<PxRaycastQueryResult*>(buffer);
buffer += raycastResultSize;
sqData->mRaycastHitBuffer = reinterpret_cast<PxRaycastHit*>(buffer);
buffer += raycastHitSize;
sqData->mSweepResults = reinterpret_cast<PxSweepQueryResult*>(buffer);
buffer += sweepResultSize;
sqData->mSweepHitBuffer = reinterpret_cast<PxSweepHit*>(buffer);
buffer += sweepHitSize;
for (PxU32 i = 0; i < maxNumWheels; i++)
{
new(sqData->mRaycastResults + i) PxRaycastQueryResult();
new(sqData->mSweepResults + i) PxSweepQueryResult();
}
for (PxU32 i = 0; i < maxNumHitPoints; i++)
{
new(sqData->mRaycastHitBuffer + i) PxRaycastHit();
new(sqData->mSweepHitBuffer + i) PxSweepHit();
}
sqData->mPreFilterShader = preFilterShader;
sqData->mPostFilterShader = postFilterShader;
return sqData;
}
void VehicleSceneQueryData::free(PxAllocatorCallback& allocator)
{
allocator.deallocate(this);
}
PxBatchQuery* VehicleSceneQueryData::setUpBatchedSceneQuery(const PxU32 batchId, const VehicleSceneQueryData& vehicleSceneQueryData, PxScene* scene)
{
const PxU32 maxNumQueriesInBatch = vehicleSceneQueryData.mNumQueriesPerBatch;
const PxU32 maxNumHitResultsInBatch = vehicleSceneQueryData.mNumQueriesPerBatch * vehicleSceneQueryData.mNumHitResultsPerQuery;
PxBatchQueryDesc sqDesc(maxNumQueriesInBatch, maxNumQueriesInBatch, 0);
sqDesc.queryMemory.userRaycastResultBuffer = vehicleSceneQueryData.mRaycastResults + batchId * maxNumQueriesInBatch;
sqDesc.queryMemory.userRaycastTouchBuffer = vehicleSceneQueryData.mRaycastHitBuffer + batchId * maxNumHitResultsInBatch;
sqDesc.queryMemory.raycastTouchBufferSize = maxNumHitResultsInBatch;
sqDesc.queryMemory.userSweepResultBuffer = vehicleSceneQueryData.mSweepResults + batchId * maxNumQueriesInBatch;
sqDesc.queryMemory.userSweepTouchBuffer = vehicleSceneQueryData.mSweepHitBuffer + batchId * maxNumHitResultsInBatch;
sqDesc.queryMemory.sweepTouchBufferSize = maxNumHitResultsInBatch;
sqDesc.preFilterShader = vehicleSceneQueryData.mPreFilterShader;
sqDesc.postFilterShader = vehicleSceneQueryData.mPostFilterShader;
return scene->createBatchQuery(sqDesc);
}
PxRaycastQueryResult* VehicleSceneQueryData::getRaycastQueryResultBuffer(const PxU32 batchId)
{
return (mRaycastResults + batchId * mNumQueriesPerBatch);
}
PxSweepQueryResult* VehicleSceneQueryData::getSweepQueryResultBuffer(const PxU32 batchId)
{
return (mSweepResults + batchId * mNumQueriesPerBatch);
}
PxU32 VehicleSceneQueryData::getQueryResultBufferSize() const
{
return mNumQueriesPerBatch;
}
static std::vector<Vertex> s_draw_lines;
static std::vector<Vertex> s_draw_points;
static PxDefaultErrorCallback gDefaultErrorCallback;
static PxDefaultAllocator gDefaultAllocatorCallback;
static PxFoundation* s_foundation;
static PxPhysics* s_physics;
static PxCooking* s_cooking;
static PxScene* s_scene;
static PxDefaultCpuDispatcher* s_cpu_dispatcher;
static PxMaterial* s_material;
static PxRigidDynamic* s_box1;
VehicleSceneQueryData* gVehicleSceneQueryData = NULL;
PxBatchQuery* gBatchQuery = NULL;
PxVehicleDrivableSurfaceToTireFrictionPairs* gFrictionPairs = NULL;
PxVehicleDrive4W* gVehicle4W = NULL;
enum
{
DRIVABLE_SURFACE = 0xffff0000,
UNDRIVABLE_SURFACE = 0x0000ffff
};
//EngineDriveVehicle gVehicle;
void setupDrivableSurface(PxFilterData& filterData)
{
filterData.word3 = static_cast<PxU32>(DRIVABLE_SURFACE);
}
void setupNonDrivableSurface(PxFilterData& filterData)
{
filterData.word3 = UNDRIVABLE_SURFACE;
}
PxQueryHitType::Enum WheelSceneQueryPreFilterBlocking
(PxFilterData filterData0, PxFilterData filterData1,
const void* constantBlock, PxU32 constantBlockSize,
PxHitFlags& queryFlags)
{
//filterData0 is the vehicle suspension query.
//filterData1 is the shape potentially hit by the query.
PX_UNUSED(filterData0);
PX_UNUSED(constantBlock);
PX_UNUSED(constantBlockSize);
PX_UNUSED(queryFlags);
return ((0 == (filterData1.word3 & DRIVABLE_SURFACE)) ? PxQueryHitType::eNONE : PxQueryHitType::eBLOCK);
}
//Drivable surface types.
enum
{
SURFACE_TYPE_TARMAC,
MAX_NUM_SURFACE_TYPES
};
//Tire types.
enum
{
TIRE_TYPE_NORMAL = 0,
TIRE_TYPE_WORN,
MAX_NUM_TIRE_TYPES
};
//Tire model friction for each combination of drivable surface type and tire type.
static PxF32 gTireFrictionMultipliers[MAX_NUM_SURFACE_TYPES][MAX_NUM_TIRE_TYPES] =
{
//NORMAL, WORN
{30.0f, 0.1f}//TARMAC
};
PxVehicleDrivableSurfaceToTireFrictionPairs* createFrictionPairs(const PxMaterial* defaultMaterial)
{
PxVehicleDrivableSurfaceType surfaceTypes[1];
surfaceTypes[0].mType = SURFACE_TYPE_TARMAC;
const PxMaterial* surfaceMaterials[1];
surfaceMaterials[0] = defaultMaterial;
PxVehicleDrivableSurfaceToTireFrictionPairs* surfaceTirePairs =
PxVehicleDrivableSurfaceToTireFrictionPairs::allocate(MAX_NUM_TIRE_TYPES, MAX_NUM_SURFACE_TYPES);
surfaceTirePairs->setup(MAX_NUM_TIRE_TYPES, MAX_NUM_SURFACE_TYPES, surfaceMaterials, surfaceTypes);
for (PxU32 i = 0; i < MAX_NUM_SURFACE_TYPES; i++)
{
for (PxU32 j = 0; j < MAX_NUM_TIRE_TYPES; j++)
{
surfaceTirePairs->setTypePairFriction(i, j, gTireFrictionMultipliers[i][j]);
}
}
return surfaceTirePairs;
}
enum
{
COLLISION_FLAG_GROUND = 1 << 0,
COLLISION_FLAG_WHEEL = 1 << 1,
COLLISION_FLAG_CHASSIS = 1 << 2,
COLLISION_FLAG_OBSTACLE = 1 << 3,
COLLISION_FLAG_DRIVABLE_OBSTACLE = 1 << 4,
COLLISION_FLAG_GROUND_AGAINST = COLLISION_FLAG_CHASSIS | COLLISION_FLAG_OBSTACLE | COLLISION_FLAG_DRIVABLE_OBSTACLE,
COLLISION_FLAG_WHEEL_AGAINST = COLLISION_FLAG_WHEEL | COLLISION_FLAG_CHASSIS | COLLISION_FLAG_OBSTACLE,
COLLISION_FLAG_CHASSIS_AGAINST = COLLISION_FLAG_GROUND | COLLISION_FLAG_WHEEL | COLLISION_FLAG_CHASSIS | COLLISION_FLAG_OBSTACLE | COLLISION_FLAG_DRIVABLE_OBSTACLE,
COLLISION_FLAG_OBSTACLE_AGAINST = COLLISION_FLAG_GROUND | COLLISION_FLAG_WHEEL | COLLISION_FLAG_CHASSIS | COLLISION_FLAG_OBSTACLE | COLLISION_FLAG_DRIVABLE_OBSTACLE,
COLLISION_FLAG_DRIVABLE_OBSTACLE_AGAINST = COLLISION_FLAG_GROUND | COLLISION_FLAG_CHASSIS | COLLISION_FLAG_OBSTACLE | COLLISION_FLAG_DRIVABLE_OBSTACLE
};
PxRigidStatic* createDrivablePlane(const PxFilterData& simFilterData, PxMaterial* material, PxPhysics* physics)
{
//Add a plane to the scene.
PxRigidStatic* groundPlane = PxCreatePlane(*physics, PxPlane(0, 1, 0, 0), *material);
//Get the plane shape so we can set query and simulation filter data.
PxShape* shapes[1];
groundPlane->getShapes(shapes, 1);
//Set the query filter data of the ground plane so that the vehicle raycasts can hit the ground.
PxFilterData qryFilterData;
setupDrivableSurface(qryFilterData);
shapes[0]->setQueryFilterData(qryFilterData);
//Set the simulation filter data of the ground plane so that it collides with the chassis of a vehicle but not the wheels.
shapes[0]->setSimulationFilterData(simFilterData);
return groundPlane;
}
//struct ActorUserData
//{
// ActorUserData()
// : vehicle(NULL),
// actor(NULL)
// {
// }
//
// const PxVehicleWheels* vehicle;
// const PxActor* actor;
//};
//
//struct ShapeUserData
//{
// ShapeUserData()
// : isWheel(false),
// wheelId(0xffffffff)
// {
// }
//
// bool isWheel;
// PxU32 wheelId;
//};
struct VehicleDesc
{
VehicleDesc()
: chassisMass(0.0f),
chassisDims(PxVec3(0.0f, 0.0f, 0.0f)),
chassisMOI(PxVec3(0.0f, 0.0f, 0.0f)),
chassisCMOffset(PxVec3(0.0f, 0.0f, 0.0f)),
chassisMaterial(NULL),
wheelMass(0.0f),
wheelWidth(0.0f),
wheelRadius(0.0f),
wheelMOI(0.0f),
wheelMaterial(NULL)
//actorUserData(NULL),
//shapeUserDatas(NULL)
{
}
PxF32 chassisMass;
PxVec3 chassisDims;
PxVec3 chassisMOI;
PxVec3 chassisCMOffset;
PxMaterial* chassisMaterial;
PxFilterData chassisSimFilterData; //word0 = collide type, word1 = collide against types, word2 = PxPairFlags
PxF32 wheelMass;
PxF32 wheelWidth;
PxF32 wheelRadius;
PxF32 wheelMOI;
PxMaterial* wheelMaterial;
PxU32 numWheels;
PxFilterData wheelSimFilterData; //word0 = collide type, word1 = collide against types, word2 = PxPairFlags
//ActorUserData* actorUserData;
//ShapeUserData* shapeUserDatas;
};
VehicleDesc initVehicleDesc()
{
//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 = 1500.0f;
const PxVec3 chassisDims(2.5f, 2.0f, 5.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);
const 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 = 4;
VehicleDesc vehicleDesc;
vehicleDesc.chassisMass = chassisMass;
vehicleDesc.chassisDims = chassisDims;
vehicleDesc.chassisMOI = chassisMOI;
vehicleDesc.chassisCMOffset = chassisCMOffset;
vehicleDesc.chassisMaterial = s_material;
vehicleDesc.chassisSimFilterData = PxFilterData(COLLISION_FLAG_CHASSIS, COLLISION_FLAG_CHASSIS_AGAINST, 0, 0);
vehicleDesc.wheelMass = wheelMass;
vehicleDesc.wheelRadius = wheelRadius;
vehicleDesc.wheelWidth = wheelWidth;
vehicleDesc.wheelMOI = wheelMOI;
vehicleDesc.numWheels = nbWheels;
vehicleDesc.wheelMaterial = s_material;
vehicleDesc.wheelSimFilterData = PxFilterData(COLLISION_FLAG_WHEEL, COLLISION_FLAG_WHEEL_AGAINST, 0, 0);
return vehicleDesc;
}
static PxConvexMesh* createConvexMesh(const PxVec3* verts, const PxU32 numVerts, PxPhysics& physics, PxCooking& cooking)
{
// Create descriptor for convex mesh
PxConvexMeshDesc convexDesc;
convexDesc.points.count = numVerts;
convexDesc.points.stride = sizeof(PxVec3);
convexDesc.points.data = verts;
convexDesc.flags = PxConvexFlag::eCOMPUTE_CONVEX;
PxConvexMesh* convexMesh = NULL;
PxDefaultMemoryOutputStream buf;
if (cooking.cookConvexMesh(convexDesc, buf))
{
PxDefaultMemoryInputData id(buf.getData(), buf.getSize());
convexMesh = physics.createConvexMesh(id);
}
return convexMesh;
}
PxConvexMesh* createWheelMesh(const PxF32 width, const PxF32 radius, PxPhysics& physics, PxCooking& cooking)
{
PxVec3 points[2 * 16];
for (PxU32 i = 0; i < 16; i++)
{
const PxF32 cosTheta = PxCos(i * PxPi * 2.0f / 16.0f);
const PxF32 sinTheta = PxSin(i * PxPi * 2.0f / 16.0f);
const PxF32 y = radius * cosTheta;
const PxF32 z = radius * sinTheta;
points[2 * i + 0] = PxVec3(-width / 2.0f, y, z);
points[2 * i + 1] = PxVec3(+width / 2.0f, y, z);
}
return createConvexMesh(points, 32, physics, cooking);
}
PxConvexMesh* createChassisMesh(const PxVec3 dims, PxPhysics& physics, PxCooking& cooking)
{
const PxF32 x = dims.x * 0.5f;
const PxF32 y = dims.y * 0.5f;
const PxF32 z = dims.z * 0.5f;
PxVec3 verts[8] =
{
PxVec3(x,y,-z),
PxVec3(x,y,z),
PxVec3(x,-y,z),
PxVec3(x,-y,-z),
PxVec3(-x,y,-z),
PxVec3(-x,y,z),
PxVec3(-x,-y,z),
PxVec3(-x,-y,-z)
};
return createConvexMesh(verts, 8, physics, cooking);
}
PxRigidDynamic* createVehicleActor
(const PxVehicleChassisData& chassisData,
PxMaterial** wheelMaterials, PxConvexMesh** wheelConvexMeshes, const PxU32 numWheels, const PxFilterData& wheelSimFilterData,
PxMaterial** chassisMaterials, PxConvexMesh** chassisConvexMeshes, const PxU32 numChassisMeshes, const PxFilterData& chassisSimFilterData,
PxPhysics& physics)
{
//We need a rigid body actor for the vehicle.
//Don't forget to add the actor to the scene after setting up the associated vehicle.
PxRigidDynamic* vehActor = physics.createRigidDynamic(PxTransform(PxIdentity));
//Wheel and chassis query filter data.
//Optional: cars don't drive on other cars.
PxFilterData wheelQryFilterData;
setupNonDrivableSurface(wheelQryFilterData);
PxFilterData chassisQryFilterData;
setupNonDrivableSurface(chassisQryFilterData);
//Add all the wheel shapes to the actor.
for (PxU32 i = 0; i < numWheels; i++)
{
PxConvexMeshGeometry geom(wheelConvexMeshes[i]);
PxShape* wheelShape = PxRigidActorExt::createExclusiveShape(*vehActor, geom, *wheelMaterials[i]);
wheelShape->setQueryFilterData(wheelQryFilterData);
wheelShape->setSimulationFilterData(wheelSimFilterData);
wheelShape->setLocalPose(PxTransform(PxIdentity));
}
//Add the chassis shapes to the actor.
for (PxU32 i = 0; i < numChassisMeshes; i++)
{
PxShape* chassisShape = PxRigidActorExt::createExclusiveShape(*vehActor, PxConvexMeshGeometry(chassisConvexMeshes[i]), *chassisMaterials[i]);
chassisShape->setQueryFilterData(chassisQryFilterData);
chassisShape->setSimulationFilterData(chassisSimFilterData);
chassisShape->setLocalPose(PxTransform(PxIdentity));
}
vehActor->setMass(chassisData.mMass);
vehActor->setMassSpaceInertiaTensor(chassisData.mMOI);
vehActor->setCMassLocalPose(PxTransform(chassisData.mCMOffset, PxQuat(PxIdentity)));
return vehActor;
}
void computeWheelCenterActorOffsets4W(const PxF32 wheelFrontZ, const PxF32 wheelRearZ, const PxVec3& chassisDims, const PxF32 wheelWidth, const PxF32 wheelRadius, const PxU32 numWheels, PxVec3* wheelCentreOffsets)
{
//chassisDims.z is the distance from the rear of the chassis to the front of the chassis.
//The front has z = 0.5*chassisDims.z and the rear has z = -0.5*chassisDims.z.
//Compute a position for the front wheel and the rear wheel along the z-axis.
//Compute the separation between each wheel along the z-axis.
const PxF32 numLeftWheels = numWheels / 2.0f;
const PxF32 deltaZ = (wheelFrontZ - wheelRearZ) / (numLeftWheels - 1.0f);
//Set the outside of the left and right wheels to be flush with the chassis.
//Set the top of the wheel to be just touching the underside of the chassis.
//Begin by setting the rear-left/rear-right/front-left,front-right wheels.
wheelCentreOffsets[PxVehicleDrive4WWheelOrder::eREAR_LEFT] = PxVec3((-chassisDims.x + wheelWidth) * 0.5f, -(chassisDims.y / 2 + wheelRadius), wheelRearZ + 0 * deltaZ * 0.5f);
wheelCentreOffsets[PxVehicleDrive4WWheelOrder::eREAR_RIGHT] = PxVec3((+chassisDims.x - wheelWidth) * 0.5f, -(chassisDims.y / 2 + wheelRadius), wheelRearZ + 0 * deltaZ * 0.5f);
wheelCentreOffsets[PxVehicleDrive4WWheelOrder::eFRONT_LEFT] = PxVec3((-chassisDims.x + wheelWidth) * 0.5f, -(chassisDims.y / 2 + wheelRadius), wheelRearZ + (numLeftWheels - 1) * deltaZ);
wheelCentreOffsets[PxVehicleDrive4WWheelOrder::eFRONT_RIGHT] = PxVec3((+chassisDims.x - wheelWidth) * 0.5f, -(chassisDims.y / 2 + wheelRadius), wheelRearZ + (numLeftWheels - 1) * deltaZ);
//Set the remaining wheels.
for (PxU32 i = 2, wheelCount = 4; i < numWheels - 2; i += 2, wheelCount += 2)
{
wheelCentreOffsets[wheelCount + 0] = PxVec3((-chassisDims.x + wheelWidth) * 0.5f, -(chassisDims.y / 2 + wheelRadius), wheelRearZ + i * deltaZ * 0.5f);
wheelCentreOffsets[wheelCount + 1] = PxVec3((+chassisDims.x - wheelWidth) * 0.5f, -(chassisDims.y / 2 + wheelRadius), wheelRearZ + i * deltaZ * 0.5f);
}
}
void setupWheelsSimulationData
(const PxF32 wheelMass, const PxF32 wheelMOI, const PxF32 wheelRadius, const PxF32 wheelWidth,
const PxU32 numWheels, const PxVec3* wheelCenterActorOffsets,
const PxVec3& chassisCMOffset, const PxF32 chassisMass,
PxVehicleWheelsSimData* wheelsSimData)
{
//Set up the wheels.
PxVehicleWheelData wheels[PX_MAX_NB_WHEELS];
{
//Set up the wheel data structures with mass, moi, radius, width.
for (PxU32 i = 0; i < numWheels; i++)
{
wheels[i].mMass = wheelMass;
wheels[i].mMOI = wheelMOI;
wheels[i].mRadius = wheelRadius;
wheels[i].mWidth = wheelWidth;
wheels[i].mMaxBrakeTorque = 2000.0f;
}
//Enable the handbrake for the rear wheels only.
wheels[PxVehicleDrive4WWheelOrder::eREAR_LEFT].mMaxHandBrakeTorque = 4000.0f;
wheels[PxVehicleDrive4WWheelOrder::eREAR_RIGHT].mMaxHandBrakeTorque = 4000.0f;
//Enable steering for the front wheels only.
wheels[PxVehicleDrive4WWheelOrder::eFRONT_LEFT].mMaxSteer = PxPi * 0.3333f;
wheels[PxVehicleDrive4WWheelOrder::eFRONT_RIGHT].mMaxSteer = PxPi * 0.3333f;
}
//Set up the tires.
PxVehicleTireData tires[PX_MAX_NB_WHEELS];
{
//Set up the tires.
for (PxU32 i = 0; i < numWheels; i++)
{
tires[i].mType = TIRE_TYPE_NORMAL;
}
}
//Set up the suspensions
PxVehicleSuspensionData suspensions[PX_MAX_NB_WHEELS];
{
//Compute the mass supported by each suspension spring.
PxF32 suspSprungMasses[PX_MAX_NB_WHEELS];
PxVehicleComputeSprungMasses
(numWheels, wheelCenterActorOffsets,
chassisCMOffset, chassisMass, 1, suspSprungMasses);
//Set the suspension data.
for (PxU32 i = 0; i < numWheels; i++)
{
suspensions[i].mMaxCompression = 0.3f;
suspensions[i].mMaxDroop = 0.1f;
suspensions[i].mSpringStrength = 35000.0f;
suspensions[i].mSpringDamperRate = 4500.0f;
suspensions[i].mSprungMass = suspSprungMasses[i];
}
//Set the camber angles.
const PxF32 camberAngleAtRest = 0.0;
const PxF32 camberAngleAtMaxDroop = 0.01f;
const PxF32 camberAngleAtMaxCompression = -0.01f;
for (PxU32 i = 0; i < numWheels; i += 2)
{
suspensions[i + 0].mCamberAtRest = camberAngleAtRest;
suspensions[i + 1].mCamberAtRest = -camberAngleAtRest;
suspensions[i + 0].mCamberAtMaxDroop = camberAngleAtMaxDroop;
suspensions[i + 1].mCamberAtMaxDroop = -camberAngleAtMaxDroop;
suspensions[i + 0].mCamberAtMaxCompression = camberAngleAtMaxCompression;
suspensions[i + 1].mCamberAtMaxCompression = -camberAngleAtMaxCompression;
}
}
//Set up the wheel geometry.
PxVec3 suspTravelDirections[PX_MAX_NB_WHEELS];
PxVec3 wheelCentreCMOffsets[PX_MAX_NB_WHEELS];
PxVec3 suspForceAppCMOffsets[PX_MAX_NB_WHEELS];
PxVec3 tireForceAppCMOffsets[PX_MAX_NB_WHEELS];
{
//Set the geometry data.
for (PxU32 i = 0; i < numWheels; i++)
{
//Vertical suspension travel.
suspTravelDirections[i] = PxVec3(0, -1, 0);
//Wheel center offset is offset from rigid body center of mass.
wheelCentreCMOffsets[i] =
wheelCenterActorOffsets[i] - chassisCMOffset;
//Suspension force application point 0.3 metres below
//rigid body center of mass.
suspForceAppCMOffsets[i] =
PxVec3(wheelCentreCMOffsets[i].x, -0.3f, wheelCentreCMOffsets[i].z);
//Tire force application point 0.3 metres below
//rigid body center of mass.
tireForceAppCMOffsets[i] =
PxVec3(wheelCentreCMOffsets[i].x, -0.3f, wheelCentreCMOffsets[i].z);
}
}
//Set up the filter data of the raycast that will be issued by each suspension.
PxFilterData qryFilterData;
setupNonDrivableSurface(qryFilterData);
//Set the wheel, tire and suspension data.
//Set the geometry data.
//Set the query filter data
for (PxU32 i = 0; i < numWheels; i++)
{
wheelsSimData->setWheelData(i, wheels[i]);
wheelsSimData->setTireData(i, tires[i]);
wheelsSimData->setSuspensionData(i, suspensions[i]);
wheelsSimData->setSuspTravelDirection(i, suspTravelDirections[i]);
wheelsSimData->setWheelCentreOffset(i, wheelCentreCMOffsets[i]);
wheelsSimData->setSuspForceAppPointOffset(i, suspForceAppCMOffsets[i]);
wheelsSimData->setTireForceAppPointOffset(i, tireForceAppCMOffsets[i]);
wheelsSimData->setSceneQueryFilterData(i, qryFilterData);
wheelsSimData->setWheelShapeMapping(i, PxI32(i));
}
//Add a front and rear anti-roll bar
PxVehicleAntiRollBarData barFront;
barFront.mWheel0 = PxVehicleDrive4WWheelOrder::eFRONT_LEFT;
barFront.mWheel1 = PxVehicleDrive4WWheelOrder::eFRONT_RIGHT;
barFront.mStiffness = 10000.0f;
wheelsSimData->addAntiRollBarData(barFront);
PxVehicleAntiRollBarData barRear;
barRear.mWheel0 = PxVehicleDrive4WWheelOrder::eREAR_LEFT;
barRear.mWheel1 = PxVehicleDrive4WWheelOrder::eREAR_RIGHT;
barRear.mStiffness = 10000.0f;
wheelsSimData->addAntiRollBarData(barRear);
}
//void configureUserData(PxVehicleWheels* vehicle, ActorUserData* actorUserData, ShapeUserData* shapeUserDatas)
//{
// if (actorUserData)
// {
// vehicle->getRigidDynamicActor()->userData = actorUserData;
// actorUserData->vehicle = vehicle;
// }
//
// if (shapeUserDatas)
// {
// PxShape* shapes[PX_MAX_NB_WHEELS + 1];
// vehicle->getRigidDynamicActor()->getShapes(shapes, PX_MAX_NB_WHEELS + 1);
// for (PxU32 i = 0; i < vehicle->mWheelsSimData.getNbWheels(); i++)
// {
// const PxI32 shapeId = vehicle->mWheelsSimData.getWheelShapeMapping(i);
// shapes[shapeId]->userData = &shapeUserDatas[i];
// shapeUserDatas[i].isWheel = true;
// shapeUserDatas[i].wheelId = i;
// }
// }
//}
PxVehicleDrive4W* createVehicle4W(const VehicleDesc& vehicle4WDesc, PxPhysics* physics, PxCooking* cooking)
{
const PxVec3 chassisDims = vehicle4WDesc.chassisDims;
const PxF32 wheelWidth = vehicle4WDesc.wheelWidth;
const PxF32 wheelRadius = vehicle4WDesc.wheelRadius;
const PxU32 numWheels = vehicle4WDesc.numWheels;
const PxFilterData& chassisSimFilterData = vehicle4WDesc.chassisSimFilterData;
const PxFilterData& wheelSimFilterData = vehicle4WDesc.wheelSimFilterData;
//Construct a physx actor with shapes for the chassis and wheels.
//Set the rigid body mass, moment of inertia, and center of mass offset.
PxRigidDynamic* veh4WActor = NULL;
{
//Construct a convex mesh for a cylindrical wheel.
PxConvexMesh* wheelMesh = createWheelMesh(wheelWidth, wheelRadius, *physics, *cooking);
//Assume all wheels are identical for simplicity.
PxConvexMesh* wheelConvexMeshes[PX_MAX_NB_WHEELS];
PxMaterial* wheelMaterials[PX_MAX_NB_WHEELS];
//Set the meshes and materials for the driven wheels.
for (PxU32 i = PxVehicleDrive4WWheelOrder::eFRONT_LEFT; i <= PxVehicleDrive4WWheelOrder::eREAR_RIGHT; i++)
{
wheelConvexMeshes[i] = wheelMesh;
wheelMaterials[i] = vehicle4WDesc.wheelMaterial;
}
//Set the meshes and materials for the non-driven wheels
for (PxU32 i = PxVehicleDrive4WWheelOrder::eREAR_RIGHT + 1; i < numWheels; i++)
{
wheelConvexMeshes[i] = wheelMesh;
wheelMaterials[i] = vehicle4WDesc.wheelMaterial;
}
//Chassis just has a single convex shape for simplicity.
PxConvexMesh* chassisConvexMesh = createChassisMesh(chassisDims, *physics, *cooking);
PxConvexMesh* chassisConvexMeshes[1] = { chassisConvexMesh };
PxMaterial* chassisMaterials[1] = { vehicle4WDesc.chassisMaterial };
//Rigid body data.
PxVehicleChassisData rigidBodyData;
rigidBodyData.mMOI = vehicle4WDesc.chassisMOI;
rigidBodyData.mMass = vehicle4WDesc.chassisMass;
rigidBodyData.mCMOffset = vehicle4WDesc.chassisCMOffset;
veh4WActor = createVehicleActor
(rigidBodyData,
wheelMaterials, wheelConvexMeshes, numWheels, wheelSimFilterData,
chassisMaterials, chassisConvexMeshes, 1, chassisSimFilterData,
*physics);
}
//Set up the sim data for the wheels.
PxVehicleWheelsSimData* wheelsSimData = PxVehicleWheelsSimData::allocate(numWheels);
{
//Compute the wheel center offsets from the origin.
PxVec3 wheelCenterActorOffsets[PX_MAX_NB_WHEELS];
const PxF32 frontZ = chassisDims.z * 0.3f;
const PxF32 rearZ = -chassisDims.z * 0.3f;
computeWheelCenterActorOffsets4W(frontZ, rearZ, chassisDims, wheelWidth, wheelRadius, numWheels, wheelCenterActorOffsets);
//Set up the simulation data for all wheels.
setupWheelsSimulationData
(vehicle4WDesc.wheelMass, vehicle4WDesc.wheelMOI, wheelRadius, wheelWidth,
numWheels, wheelCenterActorOffsets,
vehicle4WDesc.chassisCMOffset, vehicle4WDesc.chassisMass,
wheelsSimData);
}
//Set up the sim data for the vehicle drive model.
PxVehicleDriveSimData4W driveSimData;
{
//Diff
PxVehicleDifferential4WData diff;
//diff.mType = PxVehicleDifferential4WData::eDIFF_TYPE_LS_4WD;
diff.mType = PxVehicleDifferential4WData::eDIFF_TYPE_OPEN_FRONTWD;
driveSimData.setDiffData(diff);
//Engine
PxVehicleEngineData engine;
engine.mPeakTorque = 500.0f;
engine.mMaxOmega = 600.0f;//approx 6000 rpm
driveSimData.setEngineData(engine);
//Gears
PxVehicleGearsData gears;
gears.mSwitchTime = 0.5f;
driveSimData.setGearsData(gears);
//Clutch
PxVehicleClutchData clutch;
clutch.mStrength = 10.0f;
driveSimData.setClutchData(clutch);
//Ackermann steer accuracy
PxVehicleAckermannGeometryData ackermann;
ackermann.mAccuracy = 1.0f;
ackermann.mAxleSeparation =
wheelsSimData->getWheelCentreOffset(PxVehicleDrive4WWheelOrder::eFRONT_LEFT).z -
wheelsSimData->getWheelCentreOffset(PxVehicleDrive4WWheelOrder::eREAR_LEFT).z;
ackermann.mFrontWidth =
wheelsSimData->getWheelCentreOffset(PxVehicleDrive4WWheelOrder::eFRONT_RIGHT).x -
wheelsSimData->getWheelCentreOffset(PxVehicleDrive4WWheelOrder::eFRONT_LEFT).x;
ackermann.mRearWidth =
wheelsSimData->getWheelCentreOffset(PxVehicleDrive4WWheelOrder::eREAR_RIGHT).x -
wheelsSimData->getWheelCentreOffset(PxVehicleDrive4WWheelOrder::eREAR_LEFT).x;
driveSimData.setAckermannGeometryData(ackermann);
}
//Create a vehicle from the wheels and drive sim data.
PxVehicleDrive4W* vehDrive4W = PxVehicleDrive4W::allocate(numWheels);
vehDrive4W->setup(physics, veh4WActor, *wheelsSimData, driveSimData, numWheels - 4);
//Configure the userdata
//configureUserData(vehDrive4W, vehicle4WDesc.actorUserData, vehicle4WDesc.shapeUserDatas);
//Free the sim data because we don't need that any more.
wheelsSimData->free();
return vehDrive4W;
}
PxFilterFlags VehicleFilterShader
(PxFilterObjectAttributes attributes0, PxFilterData filterData0,
PxFilterObjectAttributes attributes1, PxFilterData filterData1,
PxPairFlags& pairFlags, const void* constantBlock, PxU32 constantBlockSize)
{
PX_UNUSED(attributes0);
PX_UNUSED(attributes1);
PX_UNUSED(constantBlock);
PX_UNUSED(constantBlockSize);
if ((0 == (filterData0.word0 & filterData1.word1)) && (0 == (filterData1.word0 & filterData0.word1)))
return PxFilterFlag::eSUPPRESS;
pairFlags = PxPairFlag::eCONTACT_DEFAULT;
pairFlags |= PxPairFlags(PxU16(filterData0.word2 | filterData1.word2));
return PxFilterFlags();
}
static void InitPhysics() {
s_foundation = PxCreateFoundation(PX_PHYSICS_VERSION, gDefaultAllocatorCallback, gDefaultErrorCallback);
if (!s_foundation)
exit(1);
bool recordMemoryAllocations = false;
s_physics = PxCreatePhysics(PX_PHYSICS_VERSION, *s_foundation, PxTolerancesScale(), recordMemoryAllocations, nullptr);
if (!s_physics)
exit(2);
s_cooking = PxCreateCooking(PX_PHYSICS_VERSION, *s_foundation, PxCookingParams(PxTolerancesScale()));
if (!s_cooking)
exit(3);
PxSceneDesc sceneDesc(s_physics->getTolerancesScale());
sceneDesc.gravity = PxVec3(0.0f, -9.81f, 0.0f);
// create CPU dispatcher which mNbThreads worker threads
s_cpu_dispatcher = PxDefaultCpuDispatcherCreate(1);
if (!s_cpu_dispatcher)
exit(3);
sceneDesc.cpuDispatcher = s_cpu_dispatcher;
sceneDesc.filterShader = VehicleFilterShader;
//if (!sceneDesc.filterShader)
// sceneDesc.filterShader = &PxDefaultSimulationFilterShader;
s_scene = s_physics->createScene(sceneDesc);
if (!s_scene)
exit(4);
auto m_scene = s_scene;
bool enable = true;
m_scene->setVisualizationParameter(PxVisualizationParameter::eSCALE, enable ? 1.0f : 0.0f);
m_scene->setVisualizationParameter(PxVisualizationParameter::eACTOR_AXES, enable ? 2.0f : 0.0f);
////m_scene->setVisualizationParameter(PxVisualizationParameter::eCOLLISION_EDGES, enable ? 1.0f : 0.0f);
////m_scene->setVisualizationParameter(PxVisualizationParameter::eCOLLISION_COMPOUNDS, enable ? 1.0f : 0.0f);
//m_scene->setVisualizationParameter(PxVisualizationParameter::eCOLLISION_AABBS, enable ? 1.0f : 0.0f);
//m_scene->setVisualizationParameter(PxVisualizationParameter::eMBP_REGIONS, enable ? 1.0f : 0.0f);
m_scene->setVisualizationParameter(PxVisualizationParameter::eACTOR_AXES, enable ? 0.5f : 0.0f);
//m_scene->setVisualizationParameter(PxVisualizationParameter::eBODY_ANG_VELOCITY, enable ? 1.0f : 0.0f);
//m_scene->setVisualizationParameter(PxVisualizationParameter::eBODY_AXES, enable ? 1.0f : 0.0f);
m_scene->setVisualizationParameter(PxVisualizationParameter::eBODY_LIN_VELOCITY, enable ? 1.0f : 0.0f);
//m_scene->setVisualizationParameter(PxVisualizationParameter::eBODY_MASS_AXES, enable ? 1.0f : 0.0f);
//m_scene->setVisualizationParameter(PxVisualizationParameter::eCOLLISION_AABBS, enable ? 1.0f : 0.0f);
m_scene->setVisualizationParameter(PxVisualizationParameter::eCOLLISION_AXES, enable ? 1.0f : 0.0f);
//m_scene->setVisualizationParameter(PxVisualizationParameter::eCOLLISION_COMPOUNDS, enable ? 1.0f : 0.0f);
//m_scene->setVisualizationParameter(PxVisualizationParameter::eCOLLISION_DYNAMIC, enable ? 1.0f : 0.0f);
m_scene->setVisualizationParameter(PxVisualizationParameter::eCOLLISION_EDGES, enable ? 1.0f : 0.0f);
//m_scene->setVisualizationParameter(PxVisualizationParameter::eCOLLISION_FNORMALS, enable ? 1.0f : 0.0f);
m_scene->setVisualizationParameter(PxVisualizationParameter::eCOLLISION_SHAPES, enable ? 1.0f : 0.0f);
//m_scene->setVisualizationParameter(PxVisualizationParameter::eCOLLISION_STATIC, enable ? 1.0f : 0.0f);
m_scene->setVisualizationParameter(PxVisualizationParameter::eCONTACT_ERROR, enable ? 1.0f : 0.0f);
m_scene->setVisualizationParameter(PxVisualizationParameter::eCONTACT_FORCE, enable ? 1.0f : 0.0f);
m_scene->setVisualizationParameter(PxVisualizationParameter::eCONTACT_NORMAL, enable ? 1.0f : 0.0f);
m_scene->setVisualizationParameter(PxVisualizationParameter::eCONTACT_POINT, enable ? 1.0f : 0.0f);
//m_scene->setVisualizationParameter(PxVisualizationParameter::eCULL_BOX, enable ? 1.0f : 0.0f);
//m_scene->setVisualizationParameter(PxVisualizationParameter::eFORCE_DWORD, enable ? 1.0f : 0.0f);
//m_scene->setVisualizationParameter(PxVisualizationParameter::eJOINT_LIMITS, enable ? 1.0f : 0.0f);
//m_scene->setVisualizationParameter(PxVisualizationParameter::eJOINT_LOCAL_FRAMES, enable ? 1.0f : 0.0f);
m_scene->setVisualizationParameter(PxVisualizationParameter::eWORLD_AXES, enable ? 1.0f : 0.0f);
PxShape* shape;
auto floor_material = s_physics->createMaterial(1.0f, 1.0f, 0.0f);
s_material = s_physics->createMaterial(0.5f, 0.5f, 0.6f);
//auto floor = PxCreatePlane(*s_physics, PxPlane(PxVec3(0.f, 1.f, 0.f), 0.f), *floor_material);
//floor->getShapes(&shape, 1);
//SetupFiltering(shape, CG_FLOOR, CG_BOX | CG_FLOOR, DRIVABLE_SURFACE);
//s_scene->addActor(*floor);
//s_box1 = PxCreateDynamic(*s_physics, PxTransform(PxVec3(0.f, 5.0f, 0.f)), PxBoxGeometry(0.5f, 0.5f, 0.5f), *s_material, 1.f);
//s_box1->getShapes(&shape, 1);
//SetupFiltering(shape, CG_BOX, CG_FLOOR, DRIVABLE_SURFACE);
//s_scene->addActor(*s_box1);
PxInitVehicleSDK(*s_physics);
PxVehicleSetBasisVectors(PxVec3(0, 1, 0), PxVec3(0, 0, 1));
PxVehicleSetUpdateMode(PxVehicleUpdateMode::eVELOCITY_CHANGE);
//Create the batched scene queries for the suspension raycasts.
gVehicleSceneQueryData = VehicleSceneQueryData::allocate(1, PX_MAX_NB_WHEELS, 1, 1, WheelSceneQueryPreFilterBlocking, NULL, gDefaultAllocatorCallback);
gBatchQuery = VehicleSceneQueryData::setUpBatchedSceneQuery(0, *gVehicleSceneQueryData, s_scene);
//Create the friction table for each combination of tire and surface type.
gFrictionPairs = createFrictionPairs(s_material);
//Create a plane to drive on.
PxFilterData groundPlaneSimFilterData(COLLISION_FLAG_GROUND, COLLISION_FLAG_GROUND_AGAINST, 0, 0);
auto floor = createDrivablePlane(groundPlaneSimFilterData, s_material, s_physics);
s_scene->addActor(*floor);
//Create a vehicle that will drive on the plane.
VehicleDesc vehicleDesc = initVehicleDesc();
gVehicle4W = createVehicle4W(vehicleDesc, s_physics, s_cooking);
PxTransform startTransform(PxVec3(0, (vehicleDesc.chassisDims.y * 0.5f + vehicleDesc.wheelRadius + 1.0f) + 2.0, 0), PxQuat(PxIdentity));
gVehicle4W->getRigidDynamicActor()->setGlobalPose(startTransform);
s_scene->addActor(*gVehicle4W->getRigidDynamicActor());
//Set the vehicle to rest in first gear.
//Set the vehicle to use auto-gears.
gVehicle4W->setToRestState();
gVehicle4W->mDriveDynData.forceGearChange(PxVehicleGearsData::eFIRST);
gVehicle4W->mDriveDynData.setUseAutoGears(true);
}
//PxVehicleKeySmoothingData gKeySmoothingData =
//{
// {
// 6.0f, //rise rate eANALOG_INPUT_ACCEL
// 6.0f, //rise rate eANALOG_INPUT_BRAKE
// 6.0f, //rise rate eANALOG_INPUT_HANDBRAKE
// 2.5f, //rise rate eANALOG_INPUT_STEER_LEFT
// 2.5f, //rise rate eANALOG_INPUT_STEER_RIGHT
// },
// {
// 10.0f, //fall rate eANALOG_INPUT_ACCEL
// 10.0f, //fall rate eANALOG_INPUT_BRAKE
// 10.0f, //fall rate eANALOG_INPUT_HANDBRAKE
// 5.0f, //fall rate eANALOG_INPUT_STEER_LEFT
// 5.0f //fall rate eANALOG_INPUT_STEER_RIGHT
// }
//};
PxVehicleKeySmoothingData gKeySmoothingData =
{
{
4.0f, //rise rate eANALOG_INPUT_ACCEL
10.0f, //rise rate eANALOG_INPUT_BRAKE
6.0f, //rise rate eANALOG_INPUT_HANDBRAKE
5.0f, //rise rate eANALOG_INPUT_STEER_LEFT
5.0f, //rise rate eANALOG_INPUT_STEER_RIGHT
},
{
10.0f, //fall rate eANALOG_INPUT_ACCEL
10.0f, //fall rate eANALOG_INPUT_BRAKE
10.0f, //fall rate eANALOG_INPUT_HANDBRAKE
7.0f, //fall rate eANALOG_INPUT_STEER_LEFT
7.0f //fall rate eANALOG_INPUT_STEER_RIGHT
}
};
//PxF32 gSteerVsForwardSpeedData[2 * 8] =
//{
// 0.0f, 0.75f,
// 5.0f, 0.75f,
// 30.0f, 0.125f,
// 120.0f, 0.06f,
// PX_MAX_F32, PX_MAX_F32,
// PX_MAX_F32, PX_MAX_F32,
// PX_MAX_F32, PX_MAX_F32,
// PX_MAX_F32, PX_MAX_F32
//};
PxF32 gSteerVsForwardSpeedData[2 * 8] =
{
0.0f, 0.75f,
5.0f, 0.35f,
30.0f, 0.06f,
120.0f, 0.02f,
PX_MAX_F32, PX_MAX_F32,
PX_MAX_F32, PX_MAX_F32,
PX_MAX_F32, PX_MAX_F32,
PX_MAX_F32, PX_MAX_F32
};
PxFixedSizeLookupTable<8> gSteerVsForwardSpeedTable(gSteerVsForwardSpeedData, 4);
PxVehicleDrive4WRawInputData gVehicleInputData;
bool gIsVehicleInAir = true;
static TSR::Camera s_cam;
static void PhysicsFrame() {
auto timestep = 1.0f / 75.0f;
//Update the control inputs for the vehicle.
//if (gMimicKeyInputs)
//{
// PxVehicleDrive4WSmoothDigitalRawInputsAndSetAnalogInputs(gKeySmoothingData, gSteerVsForwardSpeedTable, gVehicleInputData, timestep, gIsVehicleInAir, *gVehicle4W);
//}
//else
//{
// PxVehicleDrive4WSmoothAnalogRawInputsAndSetAnalogInputs(gPadSmoothingData, gSteerVsForwardSpeedTable, gVehicleInputData, timestep, gIsVehicleInAir, *gVehicle4W);
//}
PxVehicleDrive4WSmoothDigitalRawInputsAndSetAnalogInputs(gKeySmoothingData, gSteerVsForwardSpeedTable, gVehicleInputData, timestep, gIsVehicleInAir, *gVehicle4W);
//Raycasts.
PxVehicleWheels* vehicles[1] = { gVehicle4W };
PxRaycastQueryResult* raycastResults = gVehicleSceneQueryData->getRaycastQueryResultBuffer(0);
const PxU32 raycastResultsSize = gVehicleSceneQueryData->getQueryResultBufferSize();
PxVehicleSuspensionRaycasts(gBatchQuery, 1, vehicles, raycastResultsSize, raycastResults);
//Vehicle update.
const PxVec3 grav = s_scene->getGravity();
PxWheelQueryResult wheelQueryResults[PX_MAX_NB_WHEELS];
PxVehicleWheelQueryResult vehicleQueryResults[1] = { {wheelQueryResults, gVehicle4W->mWheelsSimData.getNbWheels()} };
PxVehicleUpdates(timestep, grav, *gFrictionPairs, 1, vehicles, vehicleQueryResults);
//Work out if the vehicle is in the air.
gIsVehicleInAir = gVehicle4W->getRigidDynamicActor()->isSleeping() ? false : PxVehicleIsInAir(vehicleQueryResults[0]);
s_scene->simulate(timestep);
s_scene->fetchResults(true);
}
static glm::vec4 U32ColorToVec4(uint32_t rgba_value) {
float red = static_cast<float>((rgba_value >> 24) & 0xFF) / 255.0f;
float green = static_cast<float>((rgba_value >> 16) & 0xFF) / 255.0f;
float blue = static_cast<float>((rgba_value >> 8) & 0xFF) / 255.0f;
float alpha = static_cast<float>(rgba_value & 0xFF) / 255.0f;
return glm::vec4(red, green, blue, alpha);
}
static void DrawPhysxDebug() {
const PxRenderBuffer& rb = s_scene->getRenderBuffer();
for(PxU32 i=0; i < rb.getNbPoints(); i++)
{
const PxDebugPoint& point = rb.getPoints()[i];
// render the point
s_draw_points.push_back({ glm::vec3(point.pos.x, point.pos.y, point.pos.z), U32ColorToVec4(point.color) });
}
for(PxU32 i=0; i < rb.getNbLines(); i++)
{
const PxDebugLine& line = rb.getLines()[i];
// render the line
s_draw_lines.push_back({ glm::vec3(line.pos0.x, line.pos0.y, line.pos0.z), U32ColorToVec4(line.color0) });
s_draw_lines.push_back({ glm::vec3(line.pos1.x, line.pos1.y, line.pos1.z), U32ColorToVec4(line.color1) });
}
}
static void ShutdownPhysics() {
PxCloseVehicleSDK();
s_physics->release();
s_foundation->release();
}
static int s_count = 0;
static bool s_first_person = false;
int main() {
if (!glfwInit()) {
std::cout << "Failed to initialize GLFW\n";
return 1;
}
glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3);
glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 2);
glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE);
glfwWindowHint(GLFW_OPENGL_FORWARD_COMPAT, GL_TRUE);
glfwWindowHint(GLFW_RESIZABLE, GL_TRUE);
glfwWindowHint(GLFW_MAXIMIZED, GL_TRUE);
glfwWindowHint(GLFW_DEPTH_BITS, 0);
//glfwWindowHint(GLFW_DEPTH_BITS, 0);
glfwWindowHint(GLFW_STENCIL_BITS, 0);
glfwWindowHint(GLFW_VISIBLE, GLFW_FALSE);
//GLFWwindow* window = glfwCreateWindow(1920, 1080, "Karo", glfwGetPrimaryMonitor(), NULL);
GLFWwindow* window = glfwCreateWindow(800, 600, "Karo", NULL, NULL);
if (!window) {
std::cerr << "Failed to create window\n";
glfwTerminate();
return 1;
}
glfwMakeContextCurrent(window);
if (glewInit() != GLEW_OK) {
std::cerr << "Failed to initialize GLEW\n";
glfwTerminate();
return 1;
}
std::cout << "OpenGL version: " << glGetString(GL_VERSION) << std::endl;
GLuint shader;
shader = glCreateProgram();
GLuint vs = glCreateShader(GL_VERTEX_SHADER);
glShaderSource(vs, 1, &s_vs_src, NULL);
glCompileShader(vs);
CheckShaderCompileErrors(vs);
GLuint fs = glCreateShader(GL_FRAGMENT_SHADER);
glShaderSource(fs, 1, &s_fs_src, NULL);
glCompileShader(fs);
CheckShaderCompileErrors(fs);
glAttachShader(shader, vs);
glAttachShader(shader, fs);
glLinkProgram(shader);
CheckProgramLinkErrors(shader);
glDeleteShader(vs);
glDeleteShader(fs);
glUseProgram(shader);
GLuint u_mvp = glGetUniformLocation(shader, "u_mvp");
GLuint vao;
glGenVertexArrays(1, &vao);
glBindVertexArray(vao);
GLuint vbo;
glGenBuffers(1, &vbo);
glBindBuffer(GL_ARRAY_BUFFER, vbo);
//setup for stream draw
glBufferData(GL_ARRAY_BUFFER, 0, NULL, GL_STREAM_DRAW);
glEnableVertexAttribArray(0);
glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, sizeof(Vertex), (void*)offsetof(Vertex, pos));
glEnableVertexAttribArray(1);
glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, sizeof(Vertex), (void*)offsetof(Vertex, color));
glBindVertexArray(0);
//glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
glfwShowWindow(window);
InitPhysics();
glfwSetKeyCallback(window, [](GLFWwindow* window, int key, int scancode, int action, int mods) {
if (action != GLFW_PRESS)
return;
switch (key) {
case GLFW_KEY_ESCAPE:
glfwSetWindowShouldClose(window, true);
break;
//case GLFW_KEY_SPACE:
// s_box1->addForce(PxVec3(0.0f, 100.0f, 0.0f));
// break;
case GLFW_KEY_E:
//for (int i = 0; i < 1; i++) {
// auto newbox = PxCreateDynamic(*s_physics, PxTransform(PxVec3(0.f, 5.0f * i, 0.f)), PxBoxGeometry(0.5f, 0.5f, 0.5f), *s_material, 1.f);
// PxShape* shape;
// newbox->getShapes(&shape, 1);
// //SetupFiltering(shape, CG_BOX, CG_FLOOR | CG_BOX, 0);
// s_scene->addActor(*newbox);
// s_count++;
//}
break;
case GLFW_KEY_W:
printf("pocet: %d\n", s_count);
break;
case GLFW_KEY_V:
s_first_person = !s_first_person;
break;
default:
break;
}
});
glfwSetCursorPosCallback(window, [](GLFWwindow* window, double x, double y) {
static double prev_x = x;
static double prev_y = y;
s_cam.ProcessMouse(x - prev_x, prev_y - y);
prev_x = x;
prev_y = y;
});
//glfwSetInputMode(window, GLFW_CURSOR, GLFW_CURSOR_DISABLED);
auto KeyDown = [window](int key) {
return glfwGetKey(window, key) == GLFW_PRESS;
};
glfwSwapInterval(1);
// Setup Dear ImGui context
IMGUI_CHECKVERSION();
ImGui::CreateContext();
ImGuiIO& io = ImGui::GetIO(); (void)io;
io.ConfigFlags |= ImGuiConfigFlags_NavEnableKeyboard; // Enable Keyboard Controls
io.ConfigFlags |= ImGuiConfigFlags_NavEnableGamepad; // Enable Gamepad Controls
// Setup Dear ImGui style
ImGui::StyleColorsDark();
//ImGui::StyleColorsLight();
// Setup Platform/Renderer backends
ImGui_ImplGlfw_InitForOpenGL(window, true);
ImGui_ImplOpenGL3_Init("#version 130");
while (!glfwWindowShouldClose(window)) {
glfwPollEvents();
// Start the Dear ImGui frame
ImGui_ImplOpenGL3_NewFrame();
ImGui_ImplGlfw_NewFrame();
ImGui::NewFrame();
int width, height;
glfwGetFramebufferSize(window, &width, &height);
glViewport(0, 0, width, height);
auto pos = gVehicle4W->getRigidDynamicActor()->getGlobalPose().p;
glm::vec3 pos_glm(pos.x, pos.y + 2.0f, pos.z);
glm::mat4 proj = glm::perspective(glm::radians(60.0f), (float)width / (float)height, 0.1f, 1000.0f);
//glm::mat4 view = glm::lookAt(glm::vec3(1.0f, 10.0f, 20.0f) * 5.0f /*pos_glm*/, glm::vec3(0.0f, 0.0f, 0.0f), glm::vec3(0.0, 1.0, 0.0));
glm::mat4 view;
if (!s_first_person) {
s_cam.position = pos_glm;
s_cam.third_person_distance = 15.0f;
view = s_cam.GetViewMatrix();
}
else {
//auto trans = gVehicle4W->getRigidDynamicActor()->getGlobalPose();
auto actor = gVehicle4W->getRigidDynamicActor();
PxShape* shapes[8];
actor->getShapes(shapes, 8);
PxMat44 trans_mat = PxShapeExt::getGlobalPose(*shapes[4], *actor);
auto fd = shapes[4]->getSimulationFilterData();
glm::mat4& trans = *((glm::mat4*)(&trans_mat));
//
//shapes[0]->getLocalPose
//
//glm::vec3 position(trans.p.x, trans.p.y, trans.p.z);
//glm::quat rotation(trans.q.x, trans.q.y, trans.q.z, trans.q.w);
//glm::mat4 rotationMatrix = glm::mat4_cast(rotation);
//// Step 2: Create translation matrix
//glm::mat4 translationMatrix = glm::translate(glm::mat4(1.0f), position);
//// Step 3: Combine rotation and translation to get the final transformation matrix
//glm::mat4 transformationMatrix = translationMatrix * rotationMatrix;
s_cam.position = glm::vec3(0.0f);
//s_cam.up_vector.y = -1.0f;
view = s_cam.GetViewMatrix(true) * glm::inverse(trans);
}
auto& dyndata = gVehicle4W->mDriveDynData;
/*gVehicle4W->computeForwardSpeed()*/
//printf("e: %9.2f g: %u\n", , dyndata.getCurrentGear());
auto speed = gVehicle4W->computeForwardSpeed() * 3.6f;
auto enginespeed = dyndata.getEngineRotationSpeed() / (PxPi * 2.0) * 60.0f;
auto gear = (int)dyndata.getCurrentGear();
auto dist = glm::length(pos_glm);
static const char* gearname[] = {
"R", "N", "1", "2", "3", "4", "5"
};
ImGui::Begin("state");
ImGui::Text("dist: %.1f m", dist);
ImGui::Text("speed: %.1f km/h", speed);
ImGui::Text("rpm: %.1f", enginespeed);
ImGui::ProgressBar(enginespeed / 6000.0f);
ImGui::SliderInt("gear", &gear, 0, 6, gearname[gear]);
ImGui::End();
ImGui::ShowDemoWindow();
glm::mat4 mvp = proj * view;
glUniformMatrix4fv(u_mvp, 1, GL_FALSE, glm::value_ptr(mvp));
//glClearColor(0.2f, 0.3f, 0.3f, 1.0f);
glClear(GL_COLOR_BUFFER_BIT);
//// test data
//s_draw_verts.push_back({ glm::vec3(-1, 0, 0), glm::vec3(1.0f, 0.0f, 0.0f) });
//s_draw_verts.push_back({ glm::vec3(1, 0, 0), glm::vec3(0.0f, 1.0f, 0.0f) });
gVehicleInputData.setDigitalAccel(KeyDown(GLFW_KEY_W));
gVehicleInputData.setDigitalSteerLeft(KeyDown(GLFW_KEY_D));
gVehicleInputData.setDigitalSteerRight(KeyDown(GLFW_KEY_A));
gVehicleInputData.setDigitalBrake(KeyDown(GLFW_KEY_S));
gVehicleInputData.setDigitalHandbrake(KeyDown(GLFW_KEY_SPACE));
gVehicleInputData.setGearUp(KeyDown(GLFW_KEY_R));
gVehicleInputData.setGearDown(KeyDown(GLFW_KEY_F));
auto grid_center = glm::floor(s_cam.position * 0.2f) * 5.0f;
grid_center.y = 0.0f;
for (int i = -50; i <= 50; i++) {
s_draw_lines.push_back({ grid_center + glm::vec3(-250.0f, 0.0f, i * 5.0f), glm::vec4(0.5f, 0.5f, 0.5f, 1.0f)});
s_draw_lines.push_back({ grid_center + glm::vec3(250.0f, 0.0f, i * 5.0f), glm::vec4(0.5f, 0.5f, 0.5f, 1.0f) });
}
for (int i = -50; i <= 50; i++) {
s_draw_lines.push_back({ grid_center + glm::vec3(i * 5.0f, 0.0f, -250.0f), glm::vec4(0.5f, 0.5f, 0.5f, 1.0f) });
s_draw_lines.push_back({ grid_center + glm::vec3(i * 5.0f, 0.0f, 250.0f), glm::vec4(0.5f, 0.5f, 0.5f, 1.0f) });
}
PhysicsFrame();
DrawPhysxDebug();
glBindVertexArray(vao);
glBindBuffer(GL_ARRAY_BUFFER, vbo);
glBufferData(GL_ARRAY_BUFFER, s_draw_lines.size() * sizeof(Vertex), s_draw_lines.data(), GL_STREAM_DRAW);
glDrawArrays(GL_LINES, 0, s_draw_lines.size());
//glBufferData(GL_ARRAY_BUFFER, s_draw_points.size() * sizeof(Vertex), s_draw_points.data(), GL_STREAM_DRAW);
//glDrawArrays(GL_POINTS, 0, s_draw_points.size());
s_draw_lines.clear();
s_draw_points.clear();
ImGui::Render();
ImGui_ImplOpenGL3_RenderDrawData(ImGui::GetDrawData());
glfwSwapBuffers(window);
}
ShutdownPhysics();
}
// Run program: Ctrl + F5 or Debug > Start Without Debugging menu
// Debug program: F5 or Debug > Start Debugging menu
// Tips for Getting Started:
// 1. Use the Solution Explorer window to add/manage files
// 2. Use the Team Explorer window to connect to source control
// 3. Use the Output window to see build output and other messages
// 4. Use the Error List window to view errors
// 5. Go to Project > Add New Item to create new code files, or Project > Add Existing Item to add existing code files to the project
// 6. In the future, to open this project again, go to File > Open > Project and select the .sln file
Reference in New Issue View Git Blame Copy Permalink