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();
}
void
ColladaNode::handleSkew(domSkew *skew)
{
if(skew == NULL)
return;
domNodeRef node = getDOMElementAs<domNode>();
Vec3f rotationAxis(skew->getValue()[1],skew->getValue()[2],skew->getValue()[3]),
translationAxis(skew->getValue()[4],skew->getValue()[5],skew->getValue()[6]);
Real32 angle(skew->getValue()[0]);
if(getGlobal()->getOptions()->getFlattenNodeXForms())
{
SkewTransformationElementUnrecPtr SkewElement = SkewTransformationElement::create();
SkewElement->setRotationAxis(rotationAxis);
SkewElement->setTranslationAxis(translationAxis);
SkewElement->setAngle(angle);
setName(SkewElement, skew->getSid());
appendStackedXForm(SkewElement, node);
}
else
{
TransformUnrecPtr xform = Transform::create();
NodeUnrecPtr xformN = makeNodeFor(xform);
Matrix skewMatrix;
MatrixSkew(skewMatrix,rotationAxis, translationAxis, angle);
xform->setMatrix(skewMatrix);
if(getGlobal()->getOptions()->getCreateNameAttachments() == true &&
node->getName() != NULL )
{
std::string nodeName = node->getName();
if(skew->getSid() != NULL &&
getGlobal()->getOptions()->getFlattenNodeXForms() == false)
{
nodeName.append("." );
nodeName.append(skew->getSid());
}
setName(xformN, nodeName);
}
appendXForm(xformN);
}
}
// static
AngularConstraint* AngularConstraint::newAngularConstraint(const glm::vec3& minAngles, const glm::vec3& maxAngles) {
float minDistance2 = glm::distance2(minAngles, glm::vec3(-PI, -PI, -PI));
float maxDistance2 = glm::distance2(maxAngles, glm::vec3(PI, PI, PI));
if (minDistance2 < EPSILON && maxDistance2 < EPSILON) {
// no constraint
return NULL;
}
// count the zero length elements
glm::vec3 rangeAngles = maxAngles - minAngles;
int pivotIndex = -1;
int numZeroes = 0;
for (int i = 0; i < 3; ++i) {
if (rangeAngles[i] < EPSILON) {
++numZeroes;
} else {
pivotIndex = i;
}
}
if (numZeroes == 2) {
// this is a hinge
int forwardIndex = (pivotIndex + 1) % 3;
glm::vec3 forwardAxis(0.0f);
forwardAxis[forwardIndex] = 1.0f;
glm::vec3 rotationAxis(0.0f);
rotationAxis[pivotIndex] = 1.0f;
return new HingeConstraint(forwardAxis, rotationAxis, minAngles[pivotIndex], maxAngles[pivotIndex]);
} else if (numZeroes == 0) {
// approximate the angular limits with a cone roller
// we assume the roll is about z
glm::vec3 middleAngles = 0.5f * (maxAngles + minAngles);
glm::quat yaw = glm::angleAxis(middleAngles[1], glm::vec3(0.0f, 1.0f, 0.0f));
glm::quat pitch = glm::angleAxis(middleAngles[0], glm::vec3(1.0f, 0.0f, 0.0f));
glm::vec3 coneAxis = pitch * yaw * glm::vec3(0.0f, 0.0f, 1.0f);
// the coneAngle is half the average range of the two non-roll rotations
glm::vec3 range = maxAngles - minAngles;
float coneAngle = 0.25f * (range[0] + range[1]);
return new ConeRollerConstraint(coneAngle, coneAxis, minAngles.z, maxAngles.z);
}
return NULL;
}
QList<GLC_Point2d> glc::polygonIn2d(QList<GLC_Point3d> polygon3d)
{
const int count= polygon3d.count();
Q_ASSERT(count > 2);
// Compute face normal
const GLC_Point3d point1(polygon3d[0]);
const GLC_Point3d point2(polygon3d[1]);
const GLC_Point3d point3(polygon3d[2]);
const GLC_Vector3d edge1(point2 - point1);
const GLC_Vector3d edge2(point3 - point2);
GLC_Vector3d polygonPlaneNormal(edge1 ^ edge2);
polygonPlaneNormal.normalize();
// Create the transformation matrix
GLC_Matrix4x4 transformation;
GLC_Vector3d rotationAxis(polygonPlaneNormal ^ Z_AXIS);
if (!rotationAxis.isNull())
{
const double angle= acos(polygonPlaneNormal * Z_AXIS);
transformation.setMatRot(rotationAxis, angle);
}
QList<GLC_Point2d> subject;
// Transform polygon vertexs
for (int i=0; i < count; ++i)
{
polygon3d[i]= transformation * polygon3d[i];
// Create 2d vector
subject << polygon3d[i].toVector2d(Z_AXIS);
}
return subject;
}
// Triangulate a no convex polygon
void glc::triangulatePolygon(QList<GLuint>* pIndexList, const QList<float>& bulkList)
{
int size= pIndexList->size();
if (polygonIsConvex(pIndexList, bulkList))
{
QList<GLuint> indexList(*pIndexList);
pIndexList->clear();
for (int i= 0; i < size - 2; ++i)
{
pIndexList->append(indexList.at(0));
pIndexList->append(indexList.at(i + 1));
pIndexList->append(indexList.at(i + 2));
}
}
else
{
// Get the polygon vertice
QList<GLC_Point3d> originPoints;
QHash<int, int> indexMap;
QList<int> face;
GLC_Point3d currentPoint;
int delta= 0;
for (int i= 0; i < size; ++i)
{
const int currentIndex= pIndexList->at(i);
currentPoint= GLC_Point3d(bulkList.at(currentIndex * 3), bulkList.at(currentIndex * 3 + 1), bulkList.at(currentIndex * 3 + 2));
if (!originPoints.contains(currentPoint))
{
originPoints.append(GLC_Point3d(bulkList.at(currentIndex * 3), bulkList.at(currentIndex * 3 + 1), bulkList.at(currentIndex * 3 + 2)));
indexMap.insert(i - delta, currentIndex);
face.append(i - delta);
}
else
{
qDebug() << "Multi points";
++delta;
}
}
// Values of PindexList must be reset
pIndexList->clear();
// Update size
size= size - delta;
// Check new size
if (size < 3) return;
//-------------- Change frame to mach polygon plane
// Compute face normal
const GLC_Point3d point1(originPoints[0]);
const GLC_Point3d point2(originPoints[1]);
const GLC_Point3d point3(originPoints[2]);
const GLC_Vector3d edge1(point2 - point1);
const GLC_Vector3d edge2(point3 - point2);
GLC_Vector3d polygonPlaneNormal(edge1 ^ edge2);
polygonPlaneNormal.normalize();
// Create the transformation matrix
GLC_Matrix4x4 transformation;
GLC_Vector3d rotationAxis(polygonPlaneNormal ^ Z_AXIS);
if (!rotationAxis.isNull())
{
const double angle= acos(polygonPlaneNormal * Z_AXIS);
transformation.setMatRot(rotationAxis, angle);
}
QList<GLC_Point2d> polygon;
// Transform polygon vertexs
for (int i=0; i < size; ++i)
{
originPoints[i]= transformation * originPoints[i];
// Create 2d vector
polygon << originPoints[i].toVector2d(Z_AXIS);
}
// Create the index
QList<int> index= face;
const bool faceIsCounterclockwise= isCounterclockwiseOrdered(polygon);
if(!faceIsCounterclockwise)
{
//qDebug() << "face Is Not Counterclockwise";
const int max= size / 2;
for (int i= 0; i < max; ++i)
{
polygon.swap(i, size - 1 -i);
int temp= face[i];
face[i]= face[size - 1 - i];
face[size - 1 - i]= temp;
}
}
QList<int> tList;
triangulate(polygon, index, tList);
size= tList.size();
for (int i= 0; i < size; i+= 3)
{
// Avoid normal problem
if (faceIsCounterclockwise)
{
pIndexList->append(indexMap.value(face[tList[i]]));
pIndexList->append(indexMap.value(face[tList[i + 1]]));
pIndexList->append(indexMap.value(face[tList[i + 2]]));
}
else
{
pIndexList->append(indexMap.value(face[tList[i + 2]]));
pIndexList->append(indexMap.value(face[tList[i + 1]]));
pIndexList->append(indexMap.value(face[tList[i]]));
}
}
Q_ASSERT(size == pIndexList->size());
}
}
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;
}
}
void Foam::cyclicGgiPolyPatch::checkDefinition() const
{
// A little bit of sanity check The rotation angle/axis is
// specified in both the master and slave patch of the
// cyclicGgi. This is a pain, but the other alternatives
// would be:
//
// - Specify in only of the two patches boundary
// definition : - which one to chose? - which default
// value to chose for the non-initialized value - Use a
// specific dictionary for this... Nope, too cumbersome.
//
// So, we impose that the boundary definition of both
// patches must specify the same information If not, well,
// we stop the simulation and ask for a fix.
if (!active())
{
// No need to check anything, the shadow is not initialized properly.
// This will happen with blockMesh when defining cyclicGGI patches.
// Return quietly
return;
}
if
(
(mag(rotationAngle()) - mag(cyclicShadow().rotationAngle())) > SMALL
|| cmptSum(rotationAxis() - cyclicShadow().rotationAxis()) > SMALL
)
{
FatalErrorIn("void cyclicGgiPolyPatch::check() const")
<< " Rotation angle for patch name : "
<< name() << " is: " << rotationAngle()
<< " axis: " << rotationAxis() << nl
<< " Rotation angle for shadow patch name: "
<< shadowName() << " is: "
<< cyclicShadow().rotationAngle() << " axis: "
<< cyclicShadow().rotationAxis() << nl
<< " Both values need to be opposite in "
<< "the boundary file. "
<< abort(FatalError);
}
if
(
(mag(separationOffset() + cyclicShadow().separationOffset())) > SMALL
)
{
FatalErrorIn("void cyclicGgiPolyPatch::check() const")
<< "Separation offset for patch name : "
<< name() << " is: " << separationOffset()
<< " Separation offset for shadow patch name: "
<< shadowName() << " is: "
<< cyclicShadow().separationOffset() << " axis: "
<< " Both values need to be opposite in "
<< "the boundary file. "
<< abort(FatalError);
}
if (debug > 1 && master())
{
Info<< "Writing transformed slave patch as VTK." << nl
<< "Master: " << name()
<< " Slave: " << shadowName()
<< " Angle (master to slave): " << rotationAngle() << " deg"
<< " Axis: " << rotationAxis()
<< " Separation: " << separationOffset() << endl;
const polyMesh& mesh = boundaryMesh().mesh();
fileName fvPath(mesh.time().path()/"VTK");
mkDir(fvPath);
pointField transformedPoints = cyclicShadow().localPoints();
tensor rot = RodriguesRotation(rotationAxis_, -rotationAngle_);
transform(transformedPoints, rot, transformedPoints);
// Add separation offset to transformed points. HJ, 24/Nov/2009
transformedPoints += cyclicShadow().separationOffset();
standAlonePatch::writeVTK
(
fvPath/fileName("cyclicGgi" + name() + cyclicShadow().name()),
cyclicShadow().localFaces(),
transformedPoints
);
}
}
