void iPhysics::setMassMatrix(void* newtonBody, float32 mass, float32 Ixx, float32 Iyy, float32 Izz)
{
if (mass >= __IGOR_GRAMM__)
{
NewtonBodySetMassMatrix(static_cast<const NewtonBody*>(newtonBody), mass, Ixx, Iyy, Izz);
}
else
{
NewtonBodySetMassMatrix(static_cast<const NewtonBody*>(newtonBody), 0, 0, 0, 0);
}
}
static void AddGearAndRack (DemoEntityManager* const scene, const dVector& origin)
{
NewtonBody* const reel0 = CreateCylinder(scene, origin + dVector (0.0f, 4.0f, 0.0f), 0.25f, 4.0f);
NewtonBody* const reel1 = CreateBox(scene, origin + dVector (0.0f, 4.0f, 2.0f), dVector(4.0f, 0.25f, 0.25f));
dMatrix matrix;
NewtonBodySetMassMatrix(reel0, 0.0f, 0.0f, 0.0f, 0.0f);
NewtonBodySetMassMatrix(reel1, 0.0f, 0.0f, 0.0f, 0.0f);
CustomHinge* const hinge0 = AddHingeWheel (scene, origin + dVector (-1.0f, 4.0f, 0.0f), 0.5f, 0.5f, reel0);
CustomHinge* const hinge1 = AddHingeWheel (scene, origin + dVector ( 1.0f, 4.0f, 0.0f), 0.5f, 0.5f, reel0);
CustomCorkScrew* const cylinder = AddCylindricalWheel(scene, origin + dVector (0.0f, 4.0f, 2.0f), 0.5f, 1.0f, reel0);
cylinder->EnableLinearLimits(true);
cylinder->SetLinearLimis(-2.0f, 2.0f);
NewtonBody* const body0 = hinge0->GetBody0();
NewtonBody* const body1 = hinge1->GetBody0();
NewtonBody* const body2 = cylinder->GetBody0();
dMatrix matrix0;
dMatrix matrix1;
dMatrix matrix2;
NewtonBodyGetMatrix (body0, &matrix0[0][0]);
NewtonBodyGetMatrix (body1, &matrix1[0][0]);
NewtonBodyGetMatrix (body2, &matrix2[0][0]);
dVector pin0 (matrix0.RotateVector(dVector( 1.0f, 0.0f, 0.0f)));
dVector pin1 (matrix1.RotateVector(dVector( 1.0f, 0.0f, 0.0f)));
dVector pin2 (matrix2.RotateVector(dVector( 1.0f, 0.0f, 0.0f)));
new CustomGear (1.0f, pin0, pin2, body0, body2);
new CustomRackAndPinion (1.0f, pin1, pin2, body1, body2);
#ifdef _USE_HARD_JOINTS
NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), reel0, NULL);
NewtonSkeletonContainerAttachBone(skeleton, hinge0->GetBody0(), reel0);
NewtonSkeletonContainerAttachBone(skeleton, hinge1->GetBody0(), reel0);
NewtonSkeletonContainerAttachBone(skeleton, cylinder->GetBody0(), reel0);
NewtonSkeletonContainerFinalize(skeleton);
#endif
// make an aggregate for disabling collisions
void* const aggregate = NewtonCollisionAggregateCreate(scene->GetNewton());
NewtonCollisionAggregateSetSelfCollision(aggregate, 0);
NewtonCollisionAggregateAddBody(aggregate, reel0);
NewtonCollisionAggregateAddBody(aggregate, reel1);
NewtonCollisionAggregateAddBody(aggregate, body0);
NewtonCollisionAggregateAddBody(aggregate, body1);
NewtonCollisionAggregateAddBody(aggregate, body2);
}
NewtonBody* CPhysics::CreateRigidBody(NewtonWorld *world, CObject3D *object, NewtonCollision *collision, float mass)
{
object->UpdateMatrix();
float matrix[16];
object->matrixModel.FlattenToArray(matrix);
Vector3 angles = object->rotation * ToRad;
NewtonBody *body = NewtonCreateBody(world, collision, matrix);
if (mass > 0)
{
float inertia[3], origin[3];
NewtonConvexCollisionCalculateInertialMatrix (collision, &inertia[0], &origin[0]);
NewtonBodySetMassMatrix(body, mass, mass * inertia[0], mass * inertia[1], mass * inertia[2]);
NewtonBodySetCentreOfMass(body, &origin[0]);
}
NewtonBodySetUserData(body, object);
NewtonBodySetTransformCallback(body, CPhysics::TransformCallback);
NewtonBodySetDestructorCallback(body, CPhysics::DestroyBodyCallback);
NewtonReleaseCollision(world, collision);
return body;
}
NewtonBody* Body::create(NewtonCollision* collision, float mass, int freezeState, const Vec4f& damping)
{
Vec4f minBox, maxBox;
Vec4f origin, inertia;
m_body = NewtonCreateBody(newton::world, collision, this->m_matrix[0]);
NewtonBodySetUserData(m_body, this);
NewtonBodySetMatrix(m_body, this->m_matrix[0]);
NewtonConvexCollisionCalculateInertialMatrix(collision, &inertia[0], &origin[0]);
if (mass < 0.0f)
mass = NewtonConvexCollisionCalculateVolume(collision) * 0.5f;
if (mass != 0.0f)
NewtonBodySetMassMatrix(m_body, mass, mass * inertia.x, mass * inertia.y, mass * inertia.z);
NewtonBodySetCentreOfMass(m_body, &origin[0]);
NewtonBodySetForceAndTorqueCallback(m_body, Body::__applyForceAndTorqueCallback);
NewtonBodySetTransformCallback(m_body, Body::__setTransformCallback);
NewtonBodySetDestructorCallback(m_body, Body::__destroyBodyCallback);
NewtonBodySetFreezeState(m_body, freezeState);
NewtonBodySetLinearDamping(m_body, damping.w);
NewtonBodySetAngularDamping(m_body, &damping[0]);
return m_body;
}
static void AddSlidingContact(DemoEntityManager* const scene, const dVector& origin)
{
// make a reel static
NewtonBody* const box0 = CreateBox(scene, origin + dVector(0.0f, 4.0f, 0.0f, 0.0f), dVector(8.0f, 0.25f, 0.25f, 0.0f));
NewtonBody* const box1 = CreateWheel(scene, origin + dVector(0.0f, 4.0f, 0.0f, 0.0f), 1.0f, 0.5f);
dMatrix matrix;
NewtonBodySetMassMatrix(box0, 0.0f, 0.0f, 0.0f, 0.0f);
// connect the bodies by a Slider joint
NewtonBodyGetMatrix(box1, &matrix[0][0]);
CustomSlidingContact* const slider = new CustomSlidingContact(matrix, box1, box0);
slider->EnableLinearLimits (true);
slider->SetLinearLimis (-4.0f, 4.0f);
// enable limit of first axis
slider->EnableAngularLimits(true);
slider->SetAngularLimis (-7.0f * 3.1416f, 5.0f * 3.1416f);
#ifdef _USE_HARD_JOINTS
NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), box0, NULL);
NewtonSkeletonContainerAttachBone(skeleton, box1, box0);
NewtonSkeletonContainerFinalize(skeleton);
#endif
}
iPhysicsBody* iPhysics::createBody(iPhysicsCollision* collisionVolume)
{
con_assert(collisionVolume != nullptr, "zero pointer");
con_assert(collisionVolume->_collision != nullptr, "zero pointer");
iaMatrixf matrix;
NewtonWaitForUpdateToFinish(static_cast<const NewtonWorld*>(_defaultWorld));
NewtonBody* newtonBody = NewtonCreateDynamicBody(static_cast<const NewtonWorld*>(_defaultWorld), static_cast<const NewtonCollision*>(collisionVolume->_collision), matrix.getData());
// set callbacks
NewtonBodySetDestructorCallback(newtonBody, reinterpret_cast<NewtonBodyDestructor>(PhysicsNodeDestructor));
NewtonBodySetTransformCallback(newtonBody, reinterpret_cast<NewtonSetTransform>(PhysicsNodeSetTransform));
NewtonBodySetForceAndTorqueCallback(newtonBody, reinterpret_cast<NewtonApplyForceAndTorque>(PhysicsApplyForceAndTorque));
NewtonBodySetMassMatrix(newtonBody, 0, 0, 0, 0);
NewtonBodySetMatrix(newtonBody, matrix.getData());
start();
iPhysicsBody* result = new iPhysicsBody(newtonBody);
_bodyListMutex.lock();
_bodys[result->getID()] = result;
_bodyListMutex.unlock();
return result;
}
static void AddBallAndSockectWithFriction (DemoEntityManager* const scene, const dVector& origin)
{
dVector size (1.0f, 1.0f, 1.0f);
NewtonBody* const base = CreateBox(scene, origin + dVector (0.0f, 5.0f + size.m_y + 0.25f, 0.0f, 0.0f), size.Scale (0.2f));
NewtonBody* const box0 = CreateCapule (scene, origin + dVector (0.0f, 5.0f, 0.0f, 0.0f), size);
NewtonBody* const box1 = CreateCapule (scene, origin + dVector (0.0f, 5.0 - size.m_y * 2.0f, 0.0f, 0.0f), size);
NewtonBodySetMassMatrix(base, 0.0f, 0.0f, 0.0f, 0.0f);
dMatrix pinMatrix (dGrammSchmidt (dVector (0.0f, -1.0f, 0.0f, 0.0f)));
// connect first box to the world
dMatrix matrix0;
NewtonBodyGetMatrix (box0, &matrix0[0][0]);
pinMatrix.m_posit = matrix0.m_posit + dVector (0.0f, size.m_y, 0.0f, 0.0f);
new CustomBallAndSocketWithFriction (pinMatrix, box0, base, 20.0f);
// link the two boxes
dMatrix matrix1;
NewtonBodyGetMatrix (box1, & matrix1[0][0]);
pinMatrix.m_posit = (matrix0.m_posit + matrix1.m_posit).Scale (0.5f);
new CustomBallAndSocketWithFriction (pinMatrix, box1, box0, 10.0f);
#ifdef _USE_HARD_JOINTS
NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), base, NULL);
NewtonSkeletonContainerAttachBone(skeleton, box0, base);
NewtonSkeletonContainerAttachBone(skeleton, box1, box0);
NewtonSkeletonContainerFinalize(skeleton);
#endif
}
static void AddCylindrical (DemoEntityManager* const scene, const dVector& origin)
{
// make a reel static
NewtonBody* const box0 = CreateCylinder (scene, origin + dVector (0.0f, 4.0f, 0.0f, 0.0f), 0.25f, 8.0f);
NewtonBody* const box1 = CreateWheel (scene, origin + dVector (0.0f, 4.0f, 0.0f, 0.0f), 1.0f, 0.5f);
dMatrix matrix;
//connect the box0 to the base by a fix joint (a hinge with zero limit)
//NewtonBodyGetMatrix(box0, &matrix[0][0]);
//CustomHinge* const fixJoint = new CustomHinge(matrix, box0, NULL);
//fixJoint->EnableLimits(true);
//fixJoint->SetLimis(0.0f, 0.0f);
NewtonBodySetMassMatrix(box0, 0.0f, 0.0f, 0.0f, 0.0f);
// connect the bodies by a CorkScrew joint
NewtonBodyGetMatrix (box1, &matrix[0][0]);
CustomCorkScrew* const cylinder = new CustomCorkScrew (matrix, box1, box0);
// enable limit of first axis
cylinder->EnableLinearLimits(true);
// set limit on second axis
cylinder->SetLinearLimis (-4.0f, 4.0f);
#ifdef _USE_HARD_JOINTS
NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), box0, NULL);
NewtonSkeletonContainerAttachBone(skeleton, box1, box0);
NewtonSkeletonContainerFinalize(skeleton);
#endif
}
static void AddGear (DemoEntityManager* const scene, const dVector& origin)
{
NewtonBody* const box0 = CreateCylinder(scene, origin + dVector (0.0f, 4.0f, 0.0f), 0.25f, 4.0f);
// this is a fix joint
NewtonBodySetMassMatrix(box0, 0.0f, 0.0f, 0.0f, 0.0f);
// connect two bodies with a hinge
CustomHinge* const hinge0 = AddHingeWheel (scene, origin + dVector (-1.0f, 4.0f, 0.0f), 0.5f, 1.0f, box0);
CustomHinge* const hinge1 = AddHingeWheel (scene, origin + dVector ( 1.0f, 4.0f, 0.0f), 0.5f, 1.0f, box0);
NewtonBody* const body0 = hinge0->GetBody0();
NewtonBody* const body1 = hinge1->GetBody0();
dMatrix matrix0;
dMatrix matrix1;
NewtonBodyGetMatrix (body0, &matrix0[0][0]);
NewtonBodyGetMatrix (body1, &matrix1[0][0]);
// relate the two body motion with a gear joint
dVector pin0 (matrix0.RotateVector(dVector (1.0f, 0.0f, 0.0f)));
dVector pin1 (matrix1.RotateVector(dVector (1.0f, 0.0f, 0.0f)));
new CustomGear (4.0f, pin0, pin1, body0, body1);
#ifdef _USE_HARD_JOINTS
NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), box0, NULL);
// NewtonSkeletonContainerAttachBone(skeleton, box0, NULL);
NewtonSkeletonContainerAttachBone(skeleton, hinge0->GetBody0(), box0);
NewtonSkeletonContainerAttachBone(skeleton, hinge1->GetBody0(), box0);
NewtonSkeletonContainerFinalize(skeleton);
#endif
}
static void Add6DOF (DemoEntityManager* const scene, const dVector& origin)
{
dVector size (1.0f, 1.0f, 1.0f);
NewtonBody* const base = CreateBox(scene, origin + dVector (0.0f, 5.0f + size.m_y + 0.25f, 0.0f, 0.0f), size.Scale (0.2f));
NewtonBody* const box0 = CreateCapule (scene, origin + dVector (0.0f, 5.0f, 0.0f, 0.0f), size);
NewtonBody* const box1 = CreateCapule (scene, origin + dVector (0.0f, 5.0 - size.m_y * 2.0f, 0.0f, 0.0f), size);
const dFloat angle = 60.0f * 3.1415592f / 180.0f;
NewtonBodySetMassMatrix(base, 0.0f, 0.0f, 0.0f, 0.0f);
dMatrix pinMatrix (dGrammSchmidt (dVector (0.0f, -1.0f, 0.0f, 0.0f)));
// connect first box to the world
dMatrix matrix0;
NewtonBodyGetMatrix (box0, & matrix0[0][0]);
pinMatrix.m_posit = matrix0.m_posit + dVector (0.0f, size.m_y, 0.0f, 0.0f);
Custom6DOF* const joint0 = new Custom6DOF (pinMatrix, pinMatrix, box0, base);
joint0->SetAngularLimits (dVector (-angle, -angle, -angle, 0.0f), dVector (angle, angle, angle, 0.0f));
// link the two boxes
dMatrix matrix1;
NewtonBodyGetMatrix (box1, &matrix1[0][0]);
pinMatrix.m_posit = (matrix0.m_posit + matrix1.m_posit).Scale (0.5f);
Custom6DOF* const joint1 = new Custom6DOF (pinMatrix, pinMatrix, box1, box0);
joint1->SetAngularLimits (dVector (-angle, -angle, -angle, 0.0f), dVector (angle, angle, angle, 0.0f));
#ifdef _USE_HARD_JOINTS
NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), base, NULL);
NewtonSkeletonContainerAttachBone(skeleton, box0, base);
NewtonSkeletonContainerAttachBone(skeleton, box1, box0);
NewtonSkeletonContainerFinalize(skeleton);
#endif
}
static void AddDistance (DemoEntityManager* const scene, const dVector& origin)
{
dVector size (1.0f, 1.0f, 1.0f);
NewtonBody* const box0 = CreateBox(scene, origin + dVector (0.0f, 5.0f + size.m_y + 0.25f, 0.0f, 0.0f), size.Scale (0.2f));
NewtonBody* const box1 = CreateCapule (scene, origin + dVector (0.0f, 5.0f, 0.0f, 0.0f), size);
NewtonBody* const box2 = CreateCapule (scene, origin + dVector (0.0f, 5.0 - size.m_y * 4.0f, 0.0f, 0.0f), size);
dMatrix pinMatrix (dGrammSchmidt (dVector (0.0f, -1.0f, 0.0f, 0.0f)));
NewtonBodySetMassMatrix(box0, 0.0f, 0.0f, 0.0f, 0.0f);
// connect first box to the world
dMatrix matrix0;
NewtonBodyGetMatrix (box1, &matrix0[0][0]);
pinMatrix.m_posit = matrix0.m_posit + dVector (0.0f, size.m_y, 0.0f, 0.0f);
new CustomBallAndSocket (pinMatrix, box1, box0);
// link the two boxes with a distance joint
dMatrix matrix1;
NewtonBodyGetMatrix (box2, &matrix1[0][0]);
// get the origins
dVector pivot0 (matrix0.m_posit - dVector (0.0f, size.m_y, 0.0f, 0.0f));
dVector pivot1 (matrix1.m_posit + dVector (0.0f, size.m_y, 0.0f, 0.0f));
// connect bodies at a corner
new CustomPointToPoint (pivot1, pivot0, box2, box1);
#ifdef _USE_HARD_JOINTS
NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), box0, NULL);
NewtonSkeletonContainerAttachBone(skeleton, box1, box0);
NewtonSkeletonContainerAttachBone(skeleton, box2, box1);
NewtonSkeletonContainerFinalize(skeleton);
#endif
}
static void AddShatterEntity (DemoEntityManager* const scene, DemoMesh* const visualMesh, NewtonCollision* const collision, const ShatterEffect& shatterEffect, dVector location)
{
dQuaternion rotation;
SimpleShatterEffectEntity* const entity = new SimpleShatterEffectEntity (visualMesh, shatterEffect);
entity->SetMatrix(*scene, rotation, location);
entity->InterpolateMatrix (*scene, 1.0f);
scene->Append(entity);
dVector origin;
dVector inertia;
NewtonConvexCollisionCalculateInertialMatrix (collision, &inertia[0], &origin[0]);
float mass = 10.0f;
int materialId = 0;
dFloat Ixx = mass * inertia[0];
dFloat Iyy = mass * inertia[1];
dFloat Izz = mass * inertia[2];
//create the rigid body
dMatrix matrix (GetIdentityMatrix());
matrix.m_posit = location;
NewtonWorld* const world = scene->GetNewton();
NewtonBody* const rigidBody = NewtonCreateBody (world, collision, &matrix[0][0]);
entity->m_myBody = rigidBody;
entity->m_myweight = dAbs (mass * DEMO_GRAVITY);
// set the correct center of gravity for this body
NewtonBodySetCentreOfMass (rigidBody, &origin[0]);
// set the mass matrix
NewtonBodySetMassMatrix (rigidBody, mass, Ixx, Iyy, Izz);
// activate
// NewtonBodyCoriolisForcesMode (blockBoxBody, 1);
// save the pointer to the graphic object with the body.
NewtonBodySetUserData (rigidBody, entity);
// assign the wood id
NewtonBodySetMaterialGroupID (rigidBody, materialId);
// set continue collision mode
// NewtonBodySetContinuousCollisionMode (rigidBody, continueCollisionMode);
// set a destructor for this rigid body
NewtonBodySetDestructorCallback (rigidBody, PhysicsBodyDestructor);
// set the transform call back function
NewtonBodySetTransformCallback (rigidBody, DemoEntity::SetTransformCallback);
// set the force and torque call back function
NewtonBodySetForceAndTorqueCallback (rigidBody, PhysicsApplyGravityForce);
}
void ScaleIntertia(NewtonBody* const body, dFloat factor) const
{
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
NewtonBodyGetMass(body, &mass, &Ixx, &Iyy, &Izz);
NewtonBodySetMassMatrix(body, mass, Ixx * factor, Iyy * factor, Izz * factor);
}
//-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~
void
PhysicsActor::setMass(float _mass)
{
// set the mass to zero to make this a static body.
// set the mass to any positive value to make this a dynamic body.
Zen::Math::Real Ixx = _mass * m_scaleX * m_scaleX;
Zen::Math::Real Iyy = _mass * m_scaleY * m_scaleY;
Zen::Math::Real Izz = _mass * m_scaleZ * m_scaleZ;
NewtonBodySetMassMatrix(m_pActor, _mass, Ixx, Iyy, Izz);
}
void AddPulley (DemoEntityManager* const scene, const dVector& origin)
{
NewtonBody* const reel0 = CreateBox(scene, origin + dVector (0.0f, 4.0f, 2.0f), dVector(4.0f, 0.25f, 0.25f));
// this is just for show
NewtonBody* const reel1 = CreateBox(scene, origin + dVector (0.0f, 4.0f, 0.0f), dVector(4.0f, 0.25f, 0.25f));
NewtonBodySetMassMatrix (reel0, 0.0f, 0.0f, 0.0f, 0.0f);
NewtonBodySetMassMatrix (reel1, 0.0f, 0.0f, 0.0f, 0.0f);
dMatrix matrix;
CustomSlider* const slider0 = AddSliderWheel (scene, origin + dVector (0.0f, 4.0f, 2.0f), 0.5f, 1.0f, reel0);
CustomSlider* const slider1 = AddSliderWheel (scene, origin + dVector (0.0f, 4.0f, 0.0f), 0.5f, 0.5f, reel0);
slider0->EnableLimits(true);
slider0->SetLimits (-2.0f, 2.0f);
NewtonBody* const body0 = slider0->GetBody0();
NewtonBody* const body1 = slider1->GetBody0();
dMatrix matrix0;
dMatrix matrix1;
NewtonBodyGetMatrix (body0, &matrix0[0][0]);
NewtonBodyGetMatrix (body1, &matrix1[0][0]);
dVector pin0 (matrix0.RotateVector(dVector (1.0f, 0.0f, 0.0f)));
dVector pin1 (matrix1.RotateVector(dVector (1.0f, 0.0f, 0.0f)));
new CustomPulley (4.0f, pin0, pin1, body0, body1);
#ifdef _USE_HARD_JOINTS
NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), reel0, NULL);
NewtonSkeletonContainerAttachBone(skeleton, slider0->GetBody0(), reel0);
NewtonSkeletonContainerAttachBone(skeleton, slider1->GetBody0(), reel0);
NewtonSkeletonContainerFinalize(skeleton);
#endif
// make an aggregate for disabling collisions
void* const aggregate = NewtonCollisionAggregateCreate (scene->GetNewton());
NewtonCollisionAggregateSetSelfCollision (aggregate, 0);
NewtonCollisionAggregateAddBody (aggregate, reel0);
NewtonCollisionAggregateAddBody (aggregate, reel1);
NewtonCollisionAggregateAddBody (aggregate, body0);
NewtonCollisionAggregateAddBody (aggregate, body1);
}
void NzPhysObject::SetMass(float mass)
{
if (m_mass > 0.f)
{
float Ix, Iy, Iz;
NewtonBodyGetMassMatrix(m_body, &m_mass, &Ix, &Iy, &Iz);
float scale = mass/m_mass;
NewtonBodySetMassMatrix(m_body, mass, Ix*scale, Iy*scale, Iz*scale);
}
else if (mass > 0.f)
{
NzVector3f inertia, origin;
m_geom->ComputeInertialMatrix(&inertia, &origin);
NewtonBodySetCentreOfMass(m_body, &origin.x);
NewtonBodySetMassMatrix(m_body, mass, inertia.x*mass, inertia.y*mass, inertia.z*mass);
NewtonBodySetForceAndTorqueCallback(m_body, &ForceAndTorqueCallback);
NewtonBodySetTransformCallback(m_body, &TransformCallback);
}
m_mass = mass;
}
void AddHinge (DemoEntityManager* const scene, const dVector& origin)
{
dVector size (1.5f, 4.0f, 0.125f);
NewtonBody* const base = CreateBox(scene, origin + dVector (-0.8f, 4.0f, 0.0f, 0.0f), dVector (0.2f, 4.0f, 0.125f));
NewtonBody* const box0 = CreateBox (scene, origin + dVector (0.0f, 4.0f, 0.0f, 0.0f), size);
NewtonBody* const box1 = CreateBox (scene, origin + dVector (1.5f, 4.0f, 0.0f, 0.0f), size);
NewtonBody* const box2 = CreateBox (scene, origin + dVector (3.0f, 4.0f, 0.0f, 0.0f), size);
NewtonBodySetMassMatrix(base, 0.0f, 0.0f, 0.0f, 0.0f);
//the joint pin is the first row of the matrix, to make a upright pin we
//take the x axis and rotate by 90 degree around the y axis
dMatrix localPin (dRollMatrix(90.0f * 3.141592f / 180.0f));
// connect first box to the world
dMatrix matrix;
NewtonBodyGetMatrix (box0, & matrix[0][0]);
matrix.m_posit += dVector (-size.m_x * 0.5f, 0.0f, 0.0f);
matrix = localPin * matrix;
// add hinge with limit and friction
CustomHinge* const hinge0 = new CustomHinge (matrix, box0, base);
hinge0->EnableLimits (true);
hinge0->SetLimits(-45.0f * 3.141592f / 180.0f, 45.0f * 3.141592f / 180.0f);
hinge0->SetFriction (20.0f);
// link the two boxes
NewtonBodyGetMatrix (box1, &matrix[0][0]);
matrix.m_posit += dVector (-size.m_x * 0.5f, 0.0f, 0.0f);
matrix = localPin * matrix;
CustomHinge* const hinge1 = new CustomHinge (matrix, box1, box0);
hinge1->EnableLimits (true);
hinge1->SetLimits (-45.0f * 3.141592f / 180.0f, 45.0f * 3.141592f / 180.0f);
hinge1->SetFriction (20.0f);
// link the two boxes
NewtonBodyGetMatrix (box2, &matrix[0][0]);
matrix.m_posit += dVector (-size.m_x * 0.5f, 0.0f, 0.0f);
matrix = localPin * matrix;
CustomHinge* const hinge2 = new CustomHinge (matrix, box2, box1);
hinge2->EnableLimits (true);
hinge2->SetLimits (-45.0f * 3.141592f / 180.0f, 45.0f * 3.141592f / 180.0f);
hinge2->SetFriction (20.0f);
#ifdef _USE_HARD_JOINTS
NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate (scene->GetNewton(), base, NULL);
NewtonSkeletonContainerAttachBone (skeleton, box0, base);
NewtonSkeletonContainerAttachBone (skeleton, box1, box0);
NewtonSkeletonContainerAttachBone (skeleton, box2, box1);
NewtonSkeletonContainerFinalize (skeleton);
#endif
}
void cPhysicsBodyNewton::SetMass(float afMass)
{
cCollideShapeNewton *pShapeNewton = static_cast<cCollideShapeNewton*>(mpShape);
cVector3f vInertia;// = pShapeNewton->GetInertia(afMass);
cVector3f vOffset;
NewtonConvexCollisionCalculateInertialMatrix(pShapeNewton->GetNewtonCollision(),
vInertia.v, vOffset.v);
vInertia = vInertia * afMass;
NewtonBodySetCentreOfMass(mpNewtonBody,vOffset.v);
NewtonBodySetMassMatrix(mpNewtonBody, afMass, vInertia.x, vInertia.y, vInertia.z);
mfMass = afMass;
}
/*
=============
CMod_PhysicsAddEntity
=============
*/
void CMod_PhysicsAddEntity(sharedEntity_t * gEnt) {
NewtonCollision* collision = NULL;
NewtonBody* body = NULL;
std::map<int, bspCmodel>::iterator it = bspModels.find (gEnt->s.modelindex);
if ( it == bspModels.end() ) {
return;
}
vec3_t inertia, com;
dMatrix matrix (GetIdentityMatrix());
bspCmodel* bmodel = &it->second;
collision = NewtonCreateConvexHull (g_world, bmodel->vertices.size(), &bmodel->vertices[0].m_x, sizeof (dVector), 0.0f, &matrix[0][0]);
body = NewtonCreateBody (g_world, collision);
NewtonConvexCollisionCalculateVolume (collision);
NewtonReleaseCollision (g_world, collision);
bmodel->rigidBody = body;
NewtonBodySetMaterialGroupID (body, defaultMaterialGroup);
NewtonBodySetUserData (body, (void*)gEnt);
NewtonBodySetDestructorCallback (body, PhysicsEntityDie);
NewtonBodySetContinuousCollisionMode (body, 0);
NewtonBodySetForceAndTorqueCallback (body, PhysicsEntityThink);
NewtonBodySetTransformCallback (body, PhysicsEntitySetTransform);
NewtonConvexCollisionCalculateInertialMatrix (collision, &inertia[0], &com[0]);
NewtonBodySetCentreOfMass (body, &com[0]);
VectorScale (inertia, 10.0f, inertia); // The inertia needs to be scaled by the mass.
NewtonBodySetMassMatrix (body, 10.f, inertia[0], inertia[1], inertia[2]);
matrix.m_posit.m_x = gEnt->s.origin[0] * UNITS_PER_METRE;
matrix.m_posit.m_y = gEnt->s.origin[1] * UNITS_PER_METRE;
matrix.m_posit.m_z = gEnt->s.origin[2] * UNITS_PER_METRE;
NewtonBodySetMatrix (body, &matrix[0][0]);
gEnt->s.pos.trType = TR_INTERPOLATE;
VectorCopy (gEnt->s.origin, gEnt->s.pos.trBase);
VectorCopy (gEnt->s.origin, gEnt->r.currentOrigin);
gEnt->s.apos.trType = TR_INTERPOLATE;
VectorCopy (gEnt->s.angles, gEnt->s.apos.trBase);
VectorCopy (gEnt->s.angles, gEnt->r.currentAngles);
}
void Newtonnode::initConvexHull(NewtonWorld *pWorld,const ion::video::Mesh& srcmesh,const float mass,
const float Ixx,const float Iyy,const float Izz)
{
if (pWorld==0) {
ion::base::log("Newtonnode::initConvexHull()",ion::base::Error) << "No NewtonWorld pointer given\n";
return;
}
if (!srcmesh.isValid()) {
ion::base::log("Newtonnode::initConvexHull()",ion::base::Error) << "Source mesh is invalid\n";
return;
}
if (!srcmesh.vertexstream().isMapped()) {
ion::base::log("Newtonnode::initConvexHull()",ion::base::Error) << "source mesh \"" << srcmesh.objIdentifier() << " is not mapped!\n";
return;
}
if ((m_pNewtonCollision!=0) && (m_pNewtonworld!=0))
NewtonReleaseCollision(m_pNewtonworld,m_pNewtonCollision);
float *pPoints;
{
pPoints=new float[3*srcmesh.vertexstream().capacity()];
for (ion_uint32 v=0;v<srcmesh.vertexstream().capacity();++v) {
const ion::math::Vector3f &rV=srcmesh.vertexstream().position(v);
pPoints[v*3+0]=rV.x();
pPoints[v*3+1]=rV.y();
pPoints[v*3+2]=rV.z();
}
}
m_pNewtonworld=pWorld;
m_pNewtonCollision=NewtonCreateConvexHull(m_pNewtonworld,srcmesh.vertexstream().capacity(),pPoints,12,0);
m_pBody=NewtonCreateBody(m_pNewtonworld,m_pNewtonCollision);
NewtonBodySetUserData(m_pBody,this);
NewtonBodySetMassMatrix(m_pBody,mass,Ixx,Iyy,Izz);
NewtonReleaseCollision(m_pNewtonworld,m_pNewtonCollision);
NewtonBodySetTransformCallback (m_pBody, physicsSetTransform);
NewtonBodySetForceAndTorqueCallback (m_pBody, physicsApplyForceAndTorque);
delete [] pPoints;
}
static void AddUniversal(DemoEntityManager* const scene, const dVector& origin)
{
dVector size(1.0f, 1.0f, 1.0f);
NewtonBody* const box0 = CreateBox(scene, origin + dVector(0.0f, 4.0f, 0.0f, 0.0f), dVector(0.25f, 0.25f, 4.0f, 0.0f));
NewtonBody* const box1 = CreateWheel(scene, origin + dVector(0.0f, 4.0f, 2.0f, 0.0f), 1.0f, 0.5f);
NewtonBody* const box2 = CreateWheel(scene, origin + dVector(0.0f, 4.0f, -2.0f, 0.0f), 1.0f, 0.5f);
NewtonBodySetMassMatrix(box0, 0.0f, 0.0f, 0.0f, 0.0f);
// align the object so that is looks nice
dMatrix matrix;
NewtonBodyGetMatrix(box1, &matrix[0][0]);
matrix = dYawMatrix (3.1416f * 0.5f) * matrix;
NewtonBodySetMatrix(box1, &matrix[0][0]);
((DemoEntity*) NewtonBodyGetUserData(box1))->ResetMatrix (*scene, matrix);
NewtonBodyGetMatrix(box2, &matrix[0][0]);
matrix = dYawMatrix(3.1416f * 0.5f) * matrix;
NewtonBodySetMatrix(box2, &matrix[0][0]);
((DemoEntity*) NewtonBodyGetUserData(box2))->ResetMatrix (*scene, matrix);
// link the two boxes
NewtonBodyGetMatrix(box1, &matrix[0][0]);
CustomUniversal* const joint1 = new CustomUniversal(matrix, box1, box0);
joint1->EnableLimit_0(true);
joint1->SetLimis_0 (-5.0f * 3.141592f, 2.0f * 3.141592f);
joint1->EnableLimit_1(true);
joint1->SetLimis_1 (-3.0f * 3.141592f, 4.0f * 3.141592f);
// link the two boxes
NewtonBodyGetMatrix(box2, &matrix[0][0]);
CustomUniversal* const joint2 = new CustomUniversal(matrix, box2, box0);
joint2->EnableLimit_0(true);
joint2->SetLimis_0 (-3.0f * 3.141592f, 5.0f * 3.141592f);
joint2->EnableLimit_1(true);
joint2->SetLimis_1(-4.0f * 3.141592f, 2.0f * 3.141592f);
#ifdef _USE_HARD_JOINTS
NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), box0, NULL);
NewtonSkeletonContainerAttachBone(skeleton, box1, box0);
NewtonSkeletonContainerAttachBone(skeleton, box2, box0);
NewtonSkeletonContainerFinalize(skeleton);
#endif
}
void makeFence(Level* level, const NewtonWorld* newtonWorld)
{
CollisionSet fencePartsCollisions;
fencePartsCollisions.insert(level->getCollision("fence"));
//fencePartsCollisions.insert(level->getCollision("fenceClip1"));
//fencePartsCollisions.insert(level->getCollision("fenceClip2"));
fencePartsCollisions.insert(level->getCollision("fenceTop"));
Collision* heightMap = level->getCollision("level");
for (size_t fencesVectorIdx = 0; fencesVectorIdx < level->m_fences.size(); fencesVectorIdx++)
{
const vector<Vector>& fence = level->m_fences[fencesVectorIdx];
for (size_t i = 0; i < fence.size() - 1; i++)
{
const Vector startPoint = fence[i];
const Vector endPoint = fence[i + 1];
Vector delta(endPoint - startPoint);
const Vector rotation(0, - delta.getRotationY(), 0);
float howMany = delta.magnitude() / fenceSpacing;
delta /= howMany;
for (int j = 0; j < howMany; j++)
{
const string bodyID = "fence" + cast<string>(fencesVectorIdx) + "_" + cast<string>(i) + "_" + cast<string>(j);
Body* body = new Body(bodyID, level, fencePartsCollisions);
if (Network::instance->m_isSingle == false)
{
NewtonBodySetMassMatrix(body->m_newtonBody, 0, 0, 0, 0);
}
body->m_soundable = true;
Vector position = Vector(startPoint + delta * static_cast<float>(j));
position.y = fenceHeight / 2 + heightMap->getHeight(position.x, position.z) + 0.05f;
body->setTransform(position, rotation);
NewtonWorldFreezeBody(newtonWorld, body->m_newtonBody);
NewtonBodySetAutoFreeze(body->m_newtonBody, 1);
level->m_bodies[bodyID] = body;
}
}
}
}
static void AddLimitedBallAndSocket (DemoEntityManager* const scene, const dVector& origin)
{
dVector size(1.0f, 1.0f, 1.0f);
NewtonBody* const base = CreateBox(scene, origin + dVector (0.0f, 5.0f + size.m_y + 0.25f, 0.0f, 0.0f), size.Scale (0.2f));
NewtonBody* const box0 = CreateCapule(scene, origin + dVector(0.0f, 5.0f, 0.0f, 0.0f), size);
NewtonBody* const box1 = CreateCapule(scene, origin + dVector(0.0f, 5.0 - size.m_y * 2.0f, 0.0f, 0.0f), size);
NewtonBody* const box2 = CreateCapule(scene, origin + dVector(0.0f, 5.0 - size.m_y * 4.0f, 0.0f, 0.0f), size);
NewtonBodySetMassMatrix(base, 0.0f, 0.0f, 0.0f, 0.0f);
dMatrix pinMatrix(dGrammSchmidt(dVector(0.0f, -1.0f, 0.0f, 0.0f)));
// connect first box to the world
dMatrix matrix;
NewtonBodyGetMatrix(box0, &matrix[0][0]);
pinMatrix.m_posit = matrix.m_posit + dVector(0.0f, size.m_y, 0.0f, 0.0f);
CustomLimitBallAndSocket* const joint0 = new CustomLimitBallAndSocket(pinMatrix, box0, base);
joint0->SetConeAngle (30.0f * 3.141592f / 180.0f);
joint0->SetTwistAngle (-30.0f * 3.141592f / 180.0f, 30.0f * 3.141592f / 180.0f);
// connect first box1 to box0 the world
NewtonBodyGetMatrix(box1, &matrix[0][0]);
pinMatrix.m_posit = matrix.m_posit + dVector(0.0f, size.m_y, 0.0f, 0.0f);
CustomLimitBallAndSocket* const joint1 = new CustomLimitBallAndSocket(pinMatrix, box1, box0);
joint1->SetConeAngle(30.0f * 3.141592f / 180.0f);
joint1->SetTwistAngle(-30.0f * 3.141592f / 180.0f, 30.0f * 3.141592f / 180.0f);
// connect first box2 to box1 the world
NewtonBodyGetMatrix(box2, &matrix[0][0]);
pinMatrix.m_posit = matrix.m_posit + dVector(0.0f, size.m_y, 0.0f, 0.0f);
CustomLimitBallAndSocket* const joint2 = new CustomLimitBallAndSocket(pinMatrix, box2, box1);
joint2->SetConeAngle(30.0f * 3.141592f / 180.0f);
joint2->SetTwistAngle(-30.0f * 3.141592f / 180.0f, 30.0f * 3.141592f / 180.0f);
#ifdef _USE_HARD_JOINTS
NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), base, NULL);
NewtonSkeletonContainerAttachBone(skeleton, box0, base);
NewtonSkeletonContainerAttachBone(skeleton, box1, box0);
NewtonSkeletonContainerAttachBone(skeleton, box2, box1);
NewtonSkeletonContainerFinalize(skeleton);
#endif
}
NewtonBody* CreateRigidBody (NewtonWorld* world, Entity* ent, NewtonCollision* collision, dFloat mass)
{
dVector minBox;
dVector maxBox;
dVector origin;
dVector inertia;
NewtonBody* body;
// Now with the collision Shape we can crate a rigid body
body = NewtonCreateBody (world, collision);
// bodies can have a destructor.
// this is a function callback that can be used to destroy any local data stored
// and that need to be destroyed before the body is destroyed.
NewtonBodySetDestructorCallback (body, DestroyBodyCallback);
// save the entity as the user data for this body
nEWTONbODySetUserData (body, ent);
// we need to set physics properties to this body
dMatrix matrix (ent->m_curRotation, ent->m_curPosition);
NewtonBodySetMatrix (body, &matrix[0][0]);
// we need to set the proper center of mass and inertia matrix for this body
// the inertia matrix calculated by this function does not include the mass.
// therefore it needs to be multiplied by the mass of the body before it is used.
NewtonConvexCollisionCalculateInertialMatrix (collision, &inertia[0], &origin[0]);
// set the body mass matrix
NewtonBodySetMassMatrix (body, mass, mass * inertia.m_x, mass * inertia.m_y, mass * inertia.m_z);
// set the body origin
NewtonBodySetCentreOfMass (body, &origin[0]);
// set the function callback to apply the external forces and torque to the body
// the most common force is Gravity
NewtonBodySetForceAndTorqueCallback (body, ApplyForceAndTorqueCallback);
// set the function callback to set the transformation state of the graphic entity associated with this body
// each time the body change position and orientation in the physics world
NewtonBodySetTransformCallback (body, SetTransformCallback);
return body;
}
void Car::initPhysics() {
NewtonWorld * nWorld = this->controller->getWorld();
NewtonCollision* collision;
float mass = 900.0f;
vector3df v1 = this->carNode->getBoundingBox().MinEdge;
vector3df v2 = this->carNode->getBoundingBox().MaxEdge;
dVector minBox(v1.X, v1.Y, v1.Z);
dVector maxBox(v2.X, v2.Y, v2.Z);
dVector size(maxBox - minBox);
dVector origin((maxBox + minBox).Scale(0.5f));
size.m_w = 1.0f;
origin.m_w = 1.0f;
dMatrix offset(GetIdentityMatrix());
offset.m_posit = origin;
collision = NewtonCreateBox(nWorld, size.m_x, size.m_y, size.m_z, 0, &offset[0][0]);
dVector inertia;
matrix4 m = this->carNode->getRelativeTransformation();
NewtonConvexHullModifierSetMatrix(collision, m.pointer());
NewtonBody * body = NewtonCreateBody(nWorld, collision, m.pointer());
NewtonBodySetUserData(body, this);
NewtonConvexCollisionCalculateInertialMatrix(collision, &inertia[0], &origin[0]);
NewtonBodySetMassMatrix(body, mass, mass * inertia.m_x, mass * inertia.m_y, mass * inertia.m_z);
NewtonBodySetCentreOfMass(body, &origin[0]);
NewtonBodySetForceAndTorqueCallback(body, applyCarMoveForce);
NewtonBodySetTransformCallback(body, applyCarTransform);
int matId = NewtonMaterialGetDefaultGroupID(nWorld);
NewtonMaterialSetCollisionCallback(nWorld, matId, matId, this, 0, applyCarCollisionForce);
NewtonReleaseCollision(nWorld, collision);
this->setCarBodyAndGravity(body, dVector(0,-10,0,0));
this->setLocalCoordinates(this->createChassisMatrix());
}
void Newtonnode::initCube(NewtonWorld *pWorld,const float xlength,const float ylength,const float zlength)
{
if (pWorld==0) {
ion::base::log("Newtonnode::initCube()",ion::base::Error) << "No NewtonWorld pointer given\n";
return;
}
if ((m_pNewtonCollision!=0) && (m_pNewtonworld!=0))
NewtonReleaseCollision(m_pNewtonworld,m_pNewtonCollision);
m_pNewtonworld=pWorld;
m_pNewtonCollision=NewtonCreateBox(m_pNewtonworld,xlength,ylength,zlength,0);
m_pBody=NewtonCreateBody(m_pNewtonworld,m_pNewtonCollision);
NewtonBodySetUserData(m_pBody,this);
NewtonBodySetMassMatrix(m_pBody, 1.0f, 1.0f, 1.0f, 1.0f);
NewtonReleaseCollision(m_pNewtonworld,m_pNewtonCollision);
NewtonBodySetTransformCallback (m_pBody, physicsSetTransform);
NewtonBodySetForceAndTorqueCallback (m_pBody, physicsApplyForceAndTorque);
}
void RigidBodyUIPane::SetSelectionMass (dFloat mass)
{
RigidBodyWorldDesc* const plugin = (RigidBodyWorldDesc*) RigidBodyWorldDesc::GetDescriptor();
Interface* const ip = GetCOREInterface();
int selectionCount = ip->GetSelNodeCount();
for (int i = 0; i < selectionCount; i ++) {
INode* const node = ip->GetSelNode(i);
RigidBodyController* const bodyInfo = (RigidBodyController*)plugin->GetRigidBodyControl(node);
if (bodyInfo) {
bodyInfo->m_mass = mass;
_ASSERTE (bodyInfo->m_body);
// dVector inertia (bodyInfo->m_inertia.Scale (mass));
// NewtonBodySetMassMatrix(bodyInfo->m_body, mass, inertia.m_x, inertia.m_y, inertia.m_z);
NewtonCollisionInfoRecord info;
NewtonCollision* const collision = NewtonBodyGetCollision(bodyInfo->m_body);
NewtonCollisionGetInfo (collision, &info);
switch (info.m_collisionType)
{
case SERIALIZE_ID_BOX:
case SERIALIZE_ID_CONE:
case SERIALIZE_ID_SPHERE:
case SERIALIZE_ID_CAPSULE:
case SERIALIZE_ID_CYLINDER:
case SERIALIZE_ID_COMPOUND:
case SERIALIZE_ID_CONVEXHULL:
case SERIALIZE_ID_CONVEXMODIFIER:
case SERIALIZE_ID_CHAMFERCYLINDER:
{
NewtonConvexCollisionCalculateInertialMatrix (collision, &bodyInfo->m_inertia[0], &bodyInfo->m_origin[0]);
NewtonBodySetCentreOfMass(bodyInfo->m_body, &bodyInfo->m_origin[0]);
NewtonBodySetMassMatrix(bodyInfo->m_body, bodyInfo->m_mass, bodyInfo->m_mass * bodyInfo->m_inertia.m_x, bodyInfo->m_mass * bodyInfo->m_inertia.m_y, bodyInfo->m_mass * bodyInfo->m_inertia.m_z);
}
}
}
}
}
void SetModelMass( dFloat mass, dAnimationJointRoot* const rootNode) const
{
dFloat volume = 0.0f;
for (dAnimationJoint* joint = GetFirstJoint(rootNode); joint; joint = GetNextJoint(joint)) {
volume += NewtonConvexCollisionCalculateVolume(NewtonBodyGetCollision(joint->GetBody()));
}
dFloat density = mass / volume;
for (dAnimationJoint* joint = GetFirstJoint(rootNode); joint; joint = GetNextJoint(joint)) {
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
NewtonBody* const body = joint->GetBody();
NewtonBodyGetMass(body, &mass, &Ixx, &Iyy, &Izz);
dFloat scale = density * NewtonConvexCollisionCalculateVolume(NewtonBodyGetCollision(body));
mass *= scale;
Ixx *= scale;
Iyy *= scale;
Izz *= scale;
NewtonBodySetMassMatrix(body, mass, Ixx, Iyy, Izz);
}
}
NewtonBody* dRigidbodyNodeInfo::CreateNewtonBody (NewtonWorld* const world, dScene* const scene, dScene::dTreeNode* const myNode) const
{
// find the collision and crate a rigid body
dAssert (IsType (dRigidbodyNodeInfo::GetRttiType()));
// attach the parent node as user data
dScene::dTreeNode* parentNode = scene->FindParentByType(myNode, dSceneNodeInfo::GetRttiType());
dSceneNodeInfo* sceneInfo = (dSceneNodeInfo*) scene->GetInfoFromNode(parentNode);
dScene::dTreeNode* shapeNode = scene->FindChildByType (myNode, dCollisionNodeInfo::GetRttiType());
dAssert (shapeNode);
dCollisionNodeInfo* collInfo = (dCollisionNodeInfo*) scene->GetInfoFromNode(shapeNode);
dMatrix matrix (sceneInfo->CalculateOrthoMatrix());
NewtonCollision* const collision = collInfo->CreateNewtonCollision (world, scene, shapeNode);
NewtonBody* const body = NewtonCreateDynamicBody(world, collision, &matrix[0][0]);
NewtonDestroyCollision(collision);
// NewtonBodySetMatrix(body, &matrix[0][0]);
NewtonBodySetUserData(body, parentNode);
NewtonBodySetCentreOfMass(body, &m_centerOfMass[0]);
NewtonBodySetMassMatrix(body, m_massMatrix.m_w, m_massMatrix.m_x, m_massMatrix.m_y, m_massMatrix.m_z);
NewtonBodySetVelocity(body, &m_velocity[0]);
NewtonBodySetOmega(body, &m_omega[0]);
//dVector internalDamp(rigidBody->GetI);
//NewtonBodySetLinearDamping(body, internalDamp);
//dVariable* bodyType = rigidBody->FindVariable("rigidBodyType");
//if (!bodyType || !strcmp (bodyType->GetString(), "default gravity")) {
// NewtonBodySetTransformCallback(body, DemoEntity::TransformCallback);
//}
return body;
}
void AddRollingBeats (NewtonWorld* nWorld)
{
dFloat mass;
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
NewtonBody* bar;
NewtonCollision* collision;
dMatrix location (GetIdentityMatrix());
location.m_posit.m_x = 5.0f;
location.m_posit.m_y = 2.0f;
location.m_posit.m_z = -2.0f;
dVector size (10.0f, 0.25f, 0.25f);
bar = NULL;
// /////////////////////////////////////////////////////////////////////////////////////
//
// create a bar and attach it to the world with a hinge with limits
//
// ////////////////////////////////////////////////////////////////////////////////////
{
CustomHinge* joint;
RenderPrimitive* visualObject;
// create the a graphic character (use a visualObject as our body
visualObject = new CylinderPrimitive (location, size.m_y, size.m_x);
// create a collision primitive to be shared by all links
collision = NewtonCreateCylinder (nWorld, size.m_y, size.m_x, NULL);
// craete the bar body
bar = NewtonCreateBody(nWorld, collision);
NewtonReleaseCollision (nWorld, collision);
// attach graphic object to the rigid body
NewtonBodySetUserData(bar, visualObject);
// set a destructor function
NewtonBodySetDestructorCallback (bar, PhysicsBodyDestructor);
// set the tranform call back function
NewtonBodySetTransformCallback (bar, PhysicsSetTransform);
// set the force and torque call back funtion
NewtonBodySetForceAndTorqueCallback (bar,PhysicsApplyGravityForce);
// calculate a acurate momenet of inertia
mass = 5.0f;
Ixx = 0.7f * mass * (size.m_y * size.m_y + size.m_z * size.m_z) / 12.0f;
Iyy = 0.7f * mass * (size.m_x * size.m_x + size.m_z * size.m_z) / 12.0f;
Izz = 0.7f * mass * (size.m_x * size.m_x + size.m_y * size.m_y) / 12.0f;
// set the mass matrix
NewtonBodySetMassMatrix (bar, mass, Ixx, Iyy, Izz);
// set the matrix for both the rigid nody and the graphic body
NewtonBodySetMatrix (bar, &location[0][0]);
PhysicsSetTransform (bar, &location[0][0]);
dVector pin (0.0f, 1.0f, 0.0f);
dVector pivot (location.m_posit);
pivot.m_x -= size.m_x * 0.5f;
// connect these two bodies by a ball and sockect joint
//joint = NewtonConstraintCreateHinge (nWorld, &pivot.m_x, &pin.m_x, link0, link1);
joint = new CustomHinge (pivot, pin, bar, NULL);
// no limits
//joint->EnableLimits (true);
//joint->SetAngleLimis (-30.0f * 3.1416f/180.0f, 30.0f * 3.1416f/180.0f);
}
{
// ////////////////////////////////////////////////////////////////////////////////////
//
// add a sliding visualObject with limits
//
NewtonBody* beat;
CustomSlider* joint;
RenderPrimitive* visualObject;
dMatrix beatLocation (location);
dVector beatSize (0.5f, 2.0f, 2.0f);
beatLocation.m_posit.m_x += size.m_x * 0.25f;
// create the a graphic character (use a visualObject as our body
visualObject = new BoxPrimitive (beatLocation, beatSize);
// create a collision primitive to be shared by all links
collision = NewtonCreateBox (nWorld, beatSize.m_x, beatSize.m_y, beatSize.m_z, NULL);
beat = NewtonCreateBody(nWorld, collision);
NewtonReleaseCollision (nWorld, collision);
// attach graphic object to the rigid body
NewtonBodySetUserData(beat, visualObject);
// set a destyuctor function
NewtonBodySetDestructorCallback (beat, PhysicsBodyDestructor);
// set the tranform call back function
NewtonBodySetTransformCallback (beat, PhysicsSetTransform);
// set the force and torque call back funtion
NewtonBodySetForceAndTorqueCallback (beat,PhysicsApplyGravityForce);
// calculate a acurate momenet of inertia
mass = 5.0f;
Ixx = 0.7f * mass * (beatSize.m_y * beatSize.m_y + beatSize.m_z * beatSize.m_z) / 12.0f;
Iyy = 0.7f * mass * (beatSize.m_x * beatSize.m_x + beatSize.m_z * beatSize.m_z) / 12.0f;
Izz = 0.7f * mass * (beatSize.m_x * beatSize.m_x + beatSize.m_y * beatSize.m_y) / 12.0f;
// set the mass matrix
NewtonBodySetMassMatrix (beat, mass, Ixx, Iyy, Izz);
// set the matrix for both the rigid nody and the graphic body
NewtonBodySetMatrix (beat, &beatLocation[0][0]);
PhysicsSetTransform (beat, &beatLocation[0][0]);
// set the pivot relative for the first bar
dVector pivot (beatLocation.m_posit);
dVector pin (beatLocation.m_front);
joint = new CustomSlider (pivot, pin, beat, bar);
// claculate the minimum and maximum limit for this joints
dFloat minLimits = ((location.m_posit.m_x - beatLocation.m_posit.m_x) - size.m_x * 0.5f);
dFloat maxLimits = ((location.m_posit.m_x - beatLocation.m_posit.m_x) + size.m_x * 0.5f);
joint->EnableLimits(true);
joint->SetLimis (minLimits, maxLimits);
}
{
// ////////////////////////////////////////////////////////////////////////////////////
//
// add a corkscrew visualObject with limits
//
// ////////////////////////////////////////////////////////////////////////////////////
NewtonBody* beat;
CustomCorkScrew* joint;
RenderPrimitive* visualObject;
dMatrix beatLocation (location);
dVector beatSize (0.5f, 1.25f, 1.25f);
beatLocation.m_posit.m_x -= size.m_x * 0.25f;
// create the a graphic character (use a visualObject as our body
//visualObject = new BoxPrimitive (beatLocation, beatSize);
visualObject = new ChamferCylinderPrimitive (beatLocation, beatSize.m_y, beatSize.m_x);
// create a collision primitive to be shared by all links
collision = NewtonCreateChamferCylinder (nWorld, beatSize.m_y, beatSize.m_x, NULL);
beat = NewtonCreateBody(nWorld, collision);
NewtonReleaseCollision (nWorld, collision);
// attach graphic object to the rigid body
NewtonBodySetUserData(beat, visualObject);
// set a destyuctor function
NewtonBodySetDestructorCallback (beat, PhysicsBodyDestructor);
// set the tranform call back function
NewtonBodySetTransformCallback (beat, PhysicsSetTransform);
// set the force and torque call back funtion
NewtonBodySetForceAndTorqueCallback (beat,PhysicsApplyGravityForce);
// calculate a acurate momenet of inertia
mass = 5.0f;
Ixx = 0.7f * mass * (beatSize.m_y * beatSize.m_y + beatSize.m_z * beatSize.m_z) / 12.0f;
Iyy = 0.7f * mass * (beatSize.m_x * beatSize.m_x + beatSize.m_z * beatSize.m_z) / 12.0f;
Izz = 0.7f * mass * (beatSize.m_x * beatSize.m_x + beatSize.m_y * beatSize.m_y) / 12.0f;
// set the mass matrix
NewtonBodySetMassMatrix (beat, mass, Ixx, Iyy, Izz);
// set the matrix for both the rigid nody and the graphic body
NewtonBodySetMatrix (beat, &beatLocation[0][0]);
PhysicsSetTransform (beat, &beatLocation[0][0]);
// set the pivot relative for the first bar
dVector pivot (beatLocation.m_posit);
dVector pin (beatLocation.m_front);
joint = new CustomCorkScrew (pivot, pin, beat, bar);
// claculate the minimum and maximum limit for this joints
dFloat minLimits = ((location.m_posit.m_x - beatLocation.m_posit.m_x) - size.m_x * 0.5f);
dFloat maxLimits = ((location.m_posit.m_x - beatLocation.m_posit.m_x) + size.m_x * 0.5f);
joint->EnableLimits(true);
joint->SetLimis (minLimits, maxLimits);
}
{
// ////////////////////////////////////////////////////////////////////////////////////
//
// add a universal joint visualObject with limits
//
// ////////////////////////////////////////////////////////////////////////////////////
NewtonBody* beat;
CustomUniversal* joint;
RenderPrimitive* visualObject;
dMatrix beatLocation (location);
dVector beatSize (0.5f, 1.25f, 1.25f);
beatLocation.m_posit.m_x -= size.m_x * 0.5f;
// create the a graphic character (use a visualObject as our body
//visualObject = new BoxPrimitive (beatLocation, beatSize);
visualObject = new ChamferCylinderPrimitive (beatLocation, beatSize.m_y, beatSize.m_x);
// create a collision primitive to be shared by all links
collision = NewtonCreateChamferCylinder (nWorld, beatSize.m_y, beatSize.m_x, NULL);
beat = NewtonCreateBody(nWorld, collision);
NewtonReleaseCollision (nWorld, collision);
// attach graphic object to the rigid body
NewtonBodySetUserData(beat, visualObject);
// set a destyuctor function
NewtonBodySetDestructorCallback (beat, PhysicsBodyDestructor);
// set the tranform call back function
NewtonBodySetTransformCallback (beat, PhysicsSetTransform);
// set the force and torque call back funtion
NewtonBodySetForceAndTorqueCallback (beat,PhysicsApplyGravityForce);
// calculate a acurate momenet of inertia
mass = 5.0f;
Ixx = 0.7f * mass * (beatSize.m_y * beatSize.m_y + beatSize.m_z * beatSize.m_z) / 12.0f;
Iyy = 0.7f * mass * (beatSize.m_x * beatSize.m_x + beatSize.m_z * beatSize.m_z) / 12.0f;
Izz = 0.7f * mass * (beatSize.m_x * beatSize.m_x + beatSize.m_y * beatSize.m_y) / 12.0f;
// set the mass matrix
NewtonBodySetMassMatrix (beat, mass, Ixx, Iyy, Izz);
// set the matrix for both the rigid nody and the graphic body
NewtonBodySetMatrix (beat, &beatLocation[0][0]);
PhysicsSetTransform (beat, &beatLocation[0][0]);
// set the pivot relative for the first bar
dVector pivot (beatLocation.m_posit);
dVector pin0 (beatLocation.m_front);
dVector pin1 (beatLocation.m_up);
// tell this joint to destroiy its local private data when destroyed
joint = new CustomUniversal (pivot, pin0, pin1, beat, bar);
}
{
// ////////////////////////////////////////////////////////////////////////////////////
//
// add a universal joint visualObject with limits
//
// ////////////////////////////////////////////////////////////////////////////////////
NewtonBody* beat;
CustomUniversal* joint;
RenderPrimitive* visualObject;
dMatrix beatLocation (location);
dVector beatSize (0.5f, 1.25f, 1.25f);
beatLocation.m_posit.m_x = size.m_x;
// create the a graphic character (use a visualObject as our body
//visualObject = new BoxPrimitive (beatLocation, beatSize);
visualObject = new ChamferCylinderPrimitive (beatLocation, beatSize.m_y, beatSize.m_x);
// create a collision primitive to be shared by all links
collision = NewtonCreateChamferCylinder (nWorld, beatSize.m_y, beatSize.m_x, NULL);
beat = NewtonCreateBody(nWorld, collision);
NewtonReleaseCollision (nWorld, collision);
// attach graphic object to the rigid body
NewtonBodySetUserData(beat, visualObject);
// set a destyuctor function
NewtonBodySetDestructorCallback (beat, PhysicsBodyDestructor);
// set the tranform call back function
NewtonBodySetTransformCallback (beat, PhysicsSetTransform);
// set the force and torque call back funtion
NewtonBodySetForceAndTorqueCallback (beat,PhysicsApplyGravityForce);
// calculate a acurate momenet of inertia
mass = 5.0f;
Ixx = 0.7f * mass * (beatSize.m_y * beatSize.m_y + beatSize.m_z * beatSize.m_z) / 12.0f;
Iyy = 0.7f * mass * (beatSize.m_x * beatSize.m_x + beatSize.m_z * beatSize.m_z) / 12.0f;
Izz = 0.7f * mass * (beatSize.m_x * beatSize.m_x + beatSize.m_y * beatSize.m_y) / 12.0f;
// set the mass matrix
NewtonBodySetMassMatrix (beat, mass, Ixx, Iyy, Izz);
// set the matrix for both the rigid nody and the graphic body
NewtonBodySetMatrix (beat, &beatLocation[0][0]);
PhysicsSetTransform (beat, &beatLocation[0][0]);
// set the pivot relative for the first bar
dVector pivot (beatLocation.m_posit);
dVector pin0 (beatLocation.m_front.Scale(-1.0f));
dVector pin1 (beatLocation.m_up);
// tell this joint to destroiy its local private data when destroyed
joint = new CustomUniversal (pivot, pin0, pin1, beat, bar);
}
}
