void Vehicle::InitPhysics()
{
hkpRigidBody* rigidBody = HK_NULL;
// Get HK Rigid Body from the entity
{
vHavokRigidBody *vRigidBody = (vHavokRigidBody *)m_chassis->Components().GetComponentOfType("vHavokRigidBody");
// Check vHavokRigidBody creation
VASSERT(vRigidBody != NULL);
// Get rigid body
rigidBody = vRigidBody->GetHkRigidBody();
VASSERT(rigidBody != HK_NULL);
}
// Get World
m_world = rigidBody->getWorld();
m_world->markForWrite();
// Create vehicle instance
{
// Setup rigid body
rigidBody->setMotionType(hkpMotion::MOTION_BOX_INERTIA);
// Specify control axis for our vehicle
hkVector4 up( 0, 0, 1);
hkVector4 forward( 1, 0, 0);
hkVector4 right( 0, -1, 0);
VehicleSetup setup(up, forward, right);
m_instance = new hkpVehicleInstance(rigidBody);
setup.buildVehicle(m_world, *m_instance);
m_instance->m_wheelCollide->addToWorld(m_world);
// Set collision filters
m_instance->getChassis()->setCollisionFilterInfo(hkpGroupFilter::calcFilterInfo(CHASSIS_LAYER, 0));
m_instance->m_wheelCollide->setCollisionFilterInfo(hkpGroupFilter::calcFilterInfo(WHEEL_LAYER, 0));
m_world->addAction(m_instance);
}
// Add reorient action
{
hkVector4 rotationAxis(1.0f, 0.0f, 0.0f);
hkVector4 upAxis(0.0f, 0.0f, 1.0f);
m_reorientAction = new hkpReorientAction(rigidBody, rotationAxis, upAxis);
}
m_world->unmarkForWrite();
}
bool BillBoard::calculateBillbaordTransform()
{
//Get camera world position
auto camera = Camera::getVisitingCamera();
const Mat4& camWorldMat = camera->getNodeToWorldTransform();
//TODO: use math lib to calculate math lib Make it easier to read and maintain
if (memcmp(_camWorldMat.m, camWorldMat.m, sizeof(float) * 16) != 0 || memcmp(_mvTransform.m, _modelViewTransform.m, sizeof(float) * 16) != 0 || _modeDirty || true)
{
//Rotate based on anchor point
Vec3 anchorPoint(_anchorPointInPoints.x , _anchorPointInPoints.y , 0.0f);
Mat4 localToWorld = _modelViewTransform;
localToWorld.translate(anchorPoint);
//Decide billboard mode
Vec3 camDir;
switch (_mode)
{
case Mode::VIEW_POINT_ORIENTED:
camDir.set(localToWorld.m[12] - camWorldMat.m[12], localToWorld.m[13] - camWorldMat.m[13], localToWorld.m[14] - camWorldMat.m[14]);
break;
case Mode::VIEW_PLANE_ORIENTED:
camWorldMat.transformVector(Vec3(0.0f, 0.0f, -1.0f), &camDir);
break;
default:
CCASSERT(false, "invalid billboard mode");
break;
}
_modeDirty = false;
if (camDir.length() < MATH_TOLERANCE)
{
camDir.set(camWorldMat.m[8], camWorldMat.m[9], camWorldMat.m[10]);
}
camDir.normalize();
Quaternion rotationQuaternion;
this->getNodeToWorldTransform().getRotation(&rotationQuaternion);
Mat4 rotationMatrix;
rotationMatrix.setIdentity();
Vec3 upAxis(rotationMatrix.m[4],rotationMatrix.m[5],rotationMatrix.m[6]);
Vec3 x, y;
camWorldMat.transformVector(upAxis, &y);
Vec3::cross(camDir, y, &x);
x.normalize();
Vec3::cross(x, camDir, &y);
y.normalize();
float xlen = sqrtf(localToWorld.m[0] * localToWorld.m[0] + localToWorld.m[1] * localToWorld.m[1] + localToWorld.m[2] * localToWorld.m[2]);
float ylen = sqrtf(localToWorld.m[4] * localToWorld.m[4] + localToWorld.m[5] * localToWorld.m[5] + localToWorld.m[6] * localToWorld.m[6]);
float zlen = sqrtf(localToWorld.m[8] * localToWorld.m[8] + localToWorld.m[9] * localToWorld.m[9] + localToWorld.m[10] * localToWorld.m[10]);
Mat4 billboardTransform;
billboardTransform.m[0] = x.x * xlen; billboardTransform.m[1] = x.y * xlen; billboardTransform.m[2] = x.z * xlen;
billboardTransform.m[4] = y.x * ylen; billboardTransform.m[5] = y.y * ylen; billboardTransform.m[6] = y.z * ylen;
billboardTransform.m[8] = -camDir.x * zlen; billboardTransform.m[9] = -camDir.y * zlen; billboardTransform.m[10] = -camDir.z * zlen;
billboardTransform.m[12] = localToWorld.m[12]; billboardTransform.m[13] = localToWorld.m[13]; billboardTransform.m[14] = localToWorld.m[14];
billboardTransform.translate(-anchorPoint);
_mvTransform = _modelViewTransform = billboardTransform;
_camWorldMat = camWorldMat;
return true;
}
return false;
}
void MakeKukaRobot(DemoEntityManager* const scene, DemoEntity* const model)
{
m_kinematicSolver = NewtonCreateInverseDynamics(scene->GetNewton());
NewtonBody* const baseFrameBody = CreateBodyPart(model, armRobotConfig[0]);
void* const baseFrameNode = NewtonInverseDynamicsAddRoot(m_kinematicSolver, baseFrameBody);
NewtonBodySetMassMatrix(baseFrameBody, 0.0f, 0.0f, 0.0f, 0.0f);
dMatrix boneAligmentMatrix(
dVector(0.0f, 1.0f, 0.0f, 0.0f),
dVector(0.0f, 0.0f, 1.0f, 0.0f),
dVector(1.0f, 0.0f, 0.0f, 0.0f),
dVector(0.0f, 0.0f, 0.0f, 1.0f));
int stackIndex = 0;
DemoEntity* childEntities[32];
void* parentBones[32];
for (DemoEntity* child = model->GetChild(); child; child = child->GetSibling()) {
parentBones[stackIndex] = baseFrameNode;
childEntities[stackIndex] = child;
stackIndex++;
}
dKukaEffector* effector = NULL;
const int partCount = sizeof(armRobotConfig) / sizeof(armRobotConfig[0]);
while (stackIndex) {
stackIndex--;
DemoEntity* const entity = childEntities[stackIndex];
void* const parentJoint = parentBones[stackIndex];
const char* const name = entity->GetName().GetStr();
for (int i = 0; i < partCount; i++) {
if (!strcmp(armRobotConfig[i].m_partName, name)) {
if (strstr(name, "bone")) {
// add a bone and all it children
dMatrix matrix;
NewtonBody* const limbBody = CreateBodyPart(entity, armRobotConfig[i]);
NewtonBodyGetMatrix(limbBody, &matrix[0][0]);
NewtonBody* const parentBody = NewtonInverseDynamicsGetBody(m_kinematicSolver, parentJoint);
dCustomInverseDynamics* const rotatingColumnHinge = new dCustomInverseDynamics(boneAligmentMatrix * matrix, limbBody, parentBody);
rotatingColumnHinge->SetJointTorque(armRobotConfig[i].m_mass * DEMO_GRAVITY * 50.0f);
rotatingColumnHinge->SetTwistAngle(armRobotConfig[i].m_minLimit * dDegreeToRad, armRobotConfig[i].m_maxLimit * dDegreeToRad);
void* const limbJoint = NewtonInverseDynamicsAddChildNode(m_kinematicSolver, parentJoint, rotatingColumnHinge->GetJoint());
for (DemoEntity* child = entity->GetChild(); child; child = child->GetSibling()) {
parentBones[stackIndex] = limbJoint;
childEntities[stackIndex] = child;
stackIndex++;
}
} else if (strstr(name, "effector")) {
// add effector
dMatrix gripperMatrix(entity->CalculateGlobalMatrix());
effector = new dKukaEffector(m_kinematicSolver, parentJoint, baseFrameBody, gripperMatrix);
effector->SetAsThreedof();
effector->SetLinearSpeed(2.0f);
effector->SetMaxLinearFriction(armRobotConfig[i].m_mass * DEMO_GRAVITY * 50.0f);
}
break;
}
}
}
// create the Animation tree for manipulation
DemoEntity* const effectoBone = model->Find("effector_arm");
dMatrix baseFrameMatrix(model->GetCurrentMatrix());
dMatrix effectorLocalMatrix(effectoBone->CalculateGlobalMatrix(model));
dVector upAxis(baseFrameMatrix.UnrotateVector(dVector(0.0f, 1.0f, 0.0f, 1.0f)));
dVector planeAxis(baseFrameMatrix.UnrotateVector(dVector(0.0f, 0.0f, 1.0f, 1.0f)));
dEffectorTreeFixPose* const fixPose = new dEffectorTreeFixPose(baseFrameBody);
m_inputModifier = new dEffectorTreeInputModifier(fixPose, upAxis, planeAxis);
m_animTreeNode = new dEffectorTreeRoot(baseFrameBody, m_inputModifier);
// set base pose
dEffectorTreeInterface::dEffectorTransform frame;
frame.m_effector = effector;
frame.m_posit = effectorLocalMatrix.m_posit;
frame.m_rotation = dQuaternion(effectorLocalMatrix);
fixPose->GetPose().Append(frame);
m_animTreeNode->GetPose().Append(frame);
NewtonInverseDynamicsEndBuild(m_kinematicSolver);
}
TruckDemo::TruckDemo(hkDemoEnvironment* env)
: CarDemo(env, false, 4, 1)
{
m_world->lock();
{
//
// Create vehicle's chassis shape.
//
hkpConvexVerticesShape* chassisShape = HK_NULL;
{
hkReal xSize = 1.9f;
hkReal xSizeFrontTop = 0.7f;
hkReal xSizeFrontMid = 1.6f;
hkReal ySize = 0.9f;
hkReal ySizeMid = ySize - 0.7f;
hkReal zSize = 1.0f;
//hkReal xBumper = 1.9f;
//hkReal yBumper = 0.35f;
//hkReal zBumper = 1.0f;
// 16 = 4 (size of "each float group", 3 for x,y,z, 1 for padding) * 4 (size of float).
int stride = sizeof(float) * 4;
{
int numVertices = 10;
float vertices[] = {
xSizeFrontTop, ySize, zSize, 0.0f, // v0
xSizeFrontTop, ySize, -zSize, 0.0f, // v1
xSize, -ySize, zSize, 0.0f, // v2
xSize, -ySize, -zSize, 0.0f, // v3
-xSize, ySize, zSize, 0.0f, // v4
-xSize, ySize, -zSize, 0.0f, // v5
-xSize, -ySize, zSize, 0.0f, // v6
-xSize, -ySize, -zSize, 0.0f, // v7
xSizeFrontMid, ySizeMid, zSize, 0.0f, // v8
xSizeFrontMid, ySizeMid, -zSize, 0.0f, // v9
};
//
// SHAPE CONSTRUCTION.
//
{
hkStridedVertices stridedVerts;
{
stridedVerts.m_numVertices = numVertices;
stridedVerts.m_striding = stride;
stridedVerts.m_vertices = vertices;
}
chassisShape = new hkpConvexVerticesShape(stridedVerts);
}
}
}
createDisplayWheels(0.5f, 0.3f);
for (int vehicleId = 0; vehicleId < m_numVehicles; vehicleId++)
{
//
// Create the vehicle.
//
{
//
// Create the chassis body.
//
hkpRigidBody* chassisRigidBody;
{
hkpRigidBodyCinfo chassisInfo;
chassisInfo.m_mass = 5000.0f;
chassisInfo.m_shape = chassisShape;
chassisInfo.m_friction = 0.8f;
chassisInfo.m_motionType = hkpMotion::MOTION_BOX_INERTIA;
// Position chassis on the ground.
chassisInfo.m_position.set(-10.0f, -4.5f, vehicleId * 5.0f);
hkpInertiaTensorComputer::setShapeVolumeMassProperties(chassisInfo.m_shape,
chassisInfo.m_mass,
chassisInfo);
chassisRigidBody = new hkpRigidBody(chassisInfo);
}
TruckSetup tsetup;
m_vehicles[vehicleId].m_vehicle = createVehicle( tsetup, chassisRigidBody);
m_vehicles[vehicleId].m_lastRPM = 0.0f;
HK_SET_OBJECT_COLOR((hkUlong)chassisRigidBody->getCollidable(), 0x80ff8080);
// This hkpAction flips the car upright if it turns over.
if (vehicleId == 0)
{
hkVector4 rotationAxis(1.0f, 0.0f, 0.0f);
hkVector4 upAxis(0.0f, 1.0f, 0.0f);
hkReal strength = 8.0f;
m_reorientAction = new hkpReorientAction(chassisRigidBody, rotationAxis, upAxis, strength);
}
chassisRigidBody->removeReference();
}
}
chassisShape->removeReference();
}
//
// Create the camera.
//
{
VehicleApiUtils::createCamera( m_camera );
}
m_world->unlock();
}
VehicleManagerDemo::VehicleManagerDemo( hkDemoEnvironment* env, hkBool createWorld, int numWheels, int numVehicles )
: hkDefaultPhysicsDemo( env )
{
const MTVehicleRayCastDemoVariant& variant = g_MTVehicleRayCastDemoVariants[m_variantId];
m_bootstrapIterations = 150;
numVehicles = 50;
m_numVehicles = numVehicles;
m_numWheels = numWheels;
m_vehicles.setSize( m_numVehicles );
setUpWorld();
if (!createWorld)
{
return;
}
m_world->lock();
//
// Create a vehicle manager and a vehicle setup object.
//
VehicleSetup* vehicleSetup;
if ( variant.m_demoType == MTVehicleRayCastDemoVariant::SINGLETHREADED_RAY_CAST || variant.m_demoType == MTVehicleRayCastDemoVariant::MULTITHREADED_RAY_CAST )
{
m_vehicleManager = new hkpVehicleRayCastBatchingManager();
vehicleSetup = new VehicleSetup();
}
else
{
m_vehicleManager = new hkpVehicleLinearCastBatchingManager();
vehicleSetup = new LinearCastVehicleSetup();
}
//
// Setup vehicle chassis and create vehicles
//
{
//
// Create vehicle's chassis shape.
//
hkpConvexVerticesShape* chassisShape = VehicleApiUtils::createCarChassisShape();
createDisplayWheels();
for ( int vehicleId = 0; vehicleId < m_vehicles.getSize(); vehicleId++ )
{
//
// Create the vehicle.
//
{
//
// Create the chassis body.
//
hkpRigidBody* chassisRigidBody;
{
hkpRigidBodyCinfo chassisInfo;
chassisInfo.m_mass = 750.0f;
chassisInfo.m_shape = chassisShape;
chassisInfo.m_friction = 0.8f;
chassisInfo.m_motionType = hkpMotion::MOTION_BOX_INERTIA;
// Position chassis on the ground.
// Inertia tensor will be set by VehicleSetupMultithreaded.
chassisInfo.m_position.set(-40.0f, -4.5f, vehicleId * 5.0f);
chassisInfo.m_inertiaTensor.setDiagonal(1.0f, 1.0f, 1.0f);
chassisInfo.m_centerOfMass.set( -0.037f, 0.143f, 0.0f);
chassisInfo.m_collisionFilterInfo = hkpGroupFilter::calcFilterInfo( CHASSIS_LAYER, 0 );
chassisRigidBody = new hkpRigidBody( chassisInfo );
}
// Create vehicle
{
hkpVehicleInstance *const vehicle = new hkpVehicleInstance( chassisRigidBody );
vehicleSetup->buildVehicle( m_world, *vehicle );
vehicle->addToWorld( m_world );
m_vehicleManager->addVehicle( vehicle );
m_vehicles[vehicleId].m_vehicle = vehicle;
m_vehicles[vehicleId].m_lastRPM = 0.0f;
m_vehicles[vehicleId].m_vehicle->m_wheelCollide->setCollisionFilterInfo( hkpGroupFilter::calcFilterInfo( WHEEL_LAYER, 0 ) );
}
// This hkpAction flips the car upright if it turns over.
if (vehicleId == 0)
{
hkVector4 rotationAxis(1.0f, 0.0f, 0.0f);
hkVector4 upAxis(0.0f, 1.0f, 0.0f);
m_reorientAction = new hkpReorientAction(chassisRigidBody, rotationAxis, upAxis);
}
chassisRigidBody->removeReference();
}
}
chassisShape->removeReference();
}
vehicleSetup->removeReference();
//
// Create the camera.
//
{
VehicleApiUtils::createCamera( m_camera );
m_followCarView = true;
}
m_world->unlock();
//
// Setup for multithreading.
//
hkpCollisionQueryJobQueueUtils::registerWithJobQueue( m_jobQueue );
// register the default addCdPoint() function; you are free to register your own implementation here though
hkpFixedBufferCdPointCollector::registerDefaultAddCdPointFunction();
// Special case for this demo variant: we do not allow the # of active SPUs to drop to zero as this can cause a deadlock.
if ( variant.m_demoType == MTVehicleRayCastDemoVariant::MULTITHREADED_RAY_CAST || variant.m_demoType == MTVehicleRayCastDemoVariant::MULTITHREADED_LINEAR_CAST )
{
m_allowZeroActiveSpus = false;
}
}
