NewtonBody* CreateRigidBody(DemoEntityManager* const scene, dFloat mass, NewtonCollision* const deformableCollision)
{
//create the rigid body
NewtonWorld* const world = scene->GetNewton();
dMatrix matrix(GetCurrentMatrix());
//matrix.m_posit.m_y = FindFloor (world, matrix.m_posit.m_x, matrix.m_posit.m_z) + 4.0f;
SetMatrix(*scene, dQuaternion(), matrix.m_posit);
SetMatrix(*scene, dQuaternion(), matrix.m_posit);
NewtonBody* const deformableBody = NewtonCreateDynamicBody(world, deformableCollision, &matrix[0][0]);
// set the mass matrix
NewtonBodySetMassProperties(deformableBody, mass, deformableCollision);
// save the pointer to the graphic object with the body.
NewtonBodySetUserData(deformableBody, this);
// assign the wood id
// NewtonBodySetMaterialGroupID (deformableBody, materialId);
// set a destructor for this rigid body
NewtonBodySetDestructorCallback(deformableBody, PhysicsBodyDestructor);
// set the transform call back function
NewtonBodySetTransformCallback(deformableBody, DemoEntity::TransformCallback);
// set the force and torque call back function
NewtonBodySetForceAndTorqueCallback(deformableBody, PhysicsApplyGravityForce);
return deformableBody;
}
void SubmitConstraints(dFloat timestep, int threadIndex)
{
CustomBallAndSocket::SubmitConstraints(timestep, threadIndex);
float invTimestep = 1.0f / timestep;
dMatrix matrix0;
dMatrix matrix1;
CalculateGlobalMatrix(matrix0, matrix1);
if (m_anim_speed != 0.0f) // some animation to illustrate purpose
{
m_anim_time += timestep * m_anim_speed;
float a0 = sin(m_anim_time);
float a1 = m_anim_offset * 3.14f;
dVector axis(sin(a1), 0.0f, cos(a1));
//dVector axis (1,0,0);
m_target = dQuaternion(axis, a0 * 0.5f);
}
// measure error
dQuaternion q0(matrix0);
dQuaternion q1(matrix1);
dQuaternion qt0 = m_target * q1;
dQuaternion qErr = ((q0.DotProduct(qt0) < 0.0f) ? dQuaternion(-q0.m_q0, q0.m_q1, q0.m_q2, q0.m_q3) : dQuaternion(q0.m_q0, -q0.m_q1, -q0.m_q2, -q0.m_q3)) * qt0;
float errorAngle = 2.0f * acos(dMax(-1.0f, dMin(1.0f, qErr.m_q0)));
dVector errorAngVel(0, 0, 0);
dMatrix basis;
if (errorAngle > 1.0e-10f) {
dVector errorAxis(qErr.m_q1, qErr.m_q2, qErr.m_q3, 0.0f);
errorAxis = errorAxis.Scale(1.0f / dSqrt(errorAxis % errorAxis));
errorAngVel = errorAxis.Scale(errorAngle * invTimestep);
basis = dGrammSchmidt(errorAxis);
} else {
basis = dMatrix(qt0, dVector(0.0f, 0.0f, 0.0f, 1.0f));
}
dVector angVel0, angVel1;
NewtonBodyGetOmega(m_body0, (float*)&angVel0);
NewtonBodyGetOmega(m_body1, (float*)&angVel1);
dVector angAcc = (errorAngVel.Scale(m_reduceError) - (angVel0 - angVel1)).Scale(invTimestep);
// motor
for (int n = 0; n < 3; n++) {
// calculate the desired acceleration
dVector &axis = basis[n];
float relAccel = angAcc % axis;
NewtonUserJointAddAngularRow(m_joint, 0.0f, &axis[0]);
NewtonUserJointSetRowAcceleration(m_joint, relAccel);
NewtonUserJointSetRowMinimumFriction(m_joint, -m_angularFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_angularFriction);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
}
}
void dNewtonBody::SetRotation(dFloat x, dFloat y, dFloat z, dFloat w)
{
dVector pos(0, 0, 0);
NewtonBodyGetPosition(m_body, &pos.m_x);
dMatrix mat(dQuaternion(x, y, z, w), pos);
NewtonBodySetMatrix(m_body, &mat[0][0]);
}
void HeightFieldCollision (DemoEntityManager* const scene)
{
// load the sky box
scene->CreateSkyBox();
CreateHeightFieldTerrain(scene, HEIGHTFIELD_DEFAULT_SIZE, HEIGHTFIELD_DEFAULT_CELLSIZE, 1.5f, 0.2f, 200.0f, -50.0f);
dMatrix locationTransform (dGetIdentityMatrix());
locationTransform.m_posit = FindFloor (scene->GetNewton(), dVector(126, 50, 50), 100.0f);
locationTransform.m_posit.m_y += 2.0f;
scene->SetCameraMatrix(dQuaternion(locationTransform), locationTransform.m_posit + dVector(0, 5, 0));
const int defaultMaterialID = NewtonMaterialGetDefaultGroupID (scene->GetNewton());
const dVector location (locationTransform.m_posit + dVector(20, 20, 0));
const dVector size (0.5f, 0.5f, 0.75f, 0.0f);
const int count = 5;
const dMatrix shapeOffsetMatrix (dGetIdentityMatrix());
AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _SPHERE_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _BOX_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _CAPSULE_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _CYLINDER_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _CONE_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
// AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _TAPERED_CAPSULE_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
// AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _TAPERED_CYLINDER_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _CHAMFER_CYLINDER_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _REGULAR_CONVEX_HULL_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _COMPOUND_CONVEX_CRUZ_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _RANDOM_CONVEX_HULL_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
}
void dNewtonBody::OnBodyTransform(const dFloat* const matrixPtr, int threadIndex)
{
dMatrix matrix(matrixPtr);
ScopeLock scopelock(&m_lock);
m_posit0 = m_posit1;
m_rotation0 = m_rotation1;
m_posit1 = matrix.m_posit;
m_rotation1 = dQuaternion(matrix);
dFloat angle = m_rotation0.DotProduct(m_rotation1);
if (angle < 0.0f) {
m_rotation1.Scale(-1.0f);
}
}
JoesRagdollJoint(NewtonBody* child, NewtonBody* parent, const dMatrix &localMatrix0, const dMatrix &localMatrix1, NewtonWorld *world)
:CustomBallAndSocket(localMatrix0, localMatrix1, child, parent)
{
m_localMatrix0 = localMatrix0;
m_localMatrix1 = localMatrix1;
m_target = dQuaternion(dVector(1.0f, 0, 0), 0.0f);
m_reduceError = 0.2f; // amount of error to reduce per timestep (more -> oszillation)
m_angularFriction = 300.0f;
m_stiffness = 0.9f;
m_anim_speed = 0.0f;
m_anim_offset = 0.0f;
m_anim_time = 0.0f;
}
void PopulateBasePose(dAnimPose& basePose, DemoEntity* const character)
{
basePose.Clear();
int stack = 1;
DemoEntity* pool[32];
pool[0] = character;
while (stack) {
stack--;
DemoEntity* const entity = pool[stack];
dAnimKeyframe& transform = basePose.Append()->GetInfo();
dMatrix matrix(entity->GetCurrentMatrix());
transform.m_posit = matrix.m_posit;
transform.m_rotation = dQuaternion(matrix);
transform.m_userData = entity;
for (DemoEntity* node = entity->GetChild(); node; node = node->GetSibling()) {
pool[stack] = node;
stack++;
}
}
}
static void CreateSimpleVoronoiShatter (DemoEntityManager* const scene)
{
// create a collision primitive
// dVector size (2.0f, 2.0f, 2.0f);
// dVector size = dVector (10.0f, 0.5f, 10.0f, 0.0f);
dVector size = dVector (5.0f, 5.0f, 5.0f, 0.0f);
NewtonWorld* const world = scene->GetNewton();
// NewtonCollision* const collision = CreateConvexCollision (world, GetIdentityMatrix(), size, _BOX_PRIMITIVE, 0);
NewtonCollision* const collision = CreateConvexCollision (world, GetIdentityMatrix(), size, _CAPSULE_PRIMITIVE, 0);
// NewtonCollision* const collision = CreateConvexCollision (world, GetIdentityMatrix(), size, _SPHERE_PRIMITIVE, 0);
// NewtonCollision* const collision = CreateConvexCollision (world, GetIdentityMatrix(), size, _REGULAR_CONVEX_HULL_PRIMITIVE, 0);
// NewtonCollision* const collision = CreateConvexCollision (world, GetIdentityMatrix(), size, _RANDOM_CONVEX_HULL_PRIMITIVE, 0);
// create a newton mesh from the collision primitive
NewtonMesh* const mesh = NewtonMeshCreateFromCollision(collision);
// apply a simple Box Mapping
int tex0 = LoadTexture("reljef.tga");
NewtonMeshApplyBoxMapping(mesh, tex0, tex0, tex0);
// pepper the bing box of the mesh with random points
dVector points[NUMBER_OF_ITERNAL_PARTS + 100];
int count = 0;
while (count < NUMBER_OF_ITERNAL_PARTS) {
dFloat x = RandomVariable(size.m_x);
dFloat y = RandomVariable(size.m_y);
dFloat z = RandomVariable(size.m_z);
if ((x <= size.m_x) && (x >= -size.m_x) && (y <= size.m_y) && (y >= -size.m_y) && (z <= size.m_z) && (z >= -size.m_z)){
points[count] = dVector (x, y, z);
count ++;
}
}
count = 4;
// Create the array of convex pieces from the mesh
int interior = LoadTexture("KAMEN-stup.tga");
// int interior = LoadTexture("camo.tga");
dMatrix textureMatrix (GetIdentityMatrix());
textureMatrix[0][0] = 1.0f / size.m_x;
textureMatrix[1][1] = 1.0f / size.m_y;
NewtonMesh* const convexParts = NewtonMeshVoronoiDecomposition (mesh, count, sizeof (dVector), &points[0].m_x, interior, &textureMatrix[0][0]);
// NewtonMesh* const convexParts = NewtonMeshConvexDecomposition (mesh, 1000000);
#if 1
dScene xxxx(world);
dScene::dTreeNode* const modelNode = xxxx.CreateSceneNode(xxxx.GetRootNode());
dScene::dTreeNode* const meshNode = xxxx.CreateMeshNode(modelNode);
dMeshNodeInfo* const modelMesh = (dMeshNodeInfo*)xxxx.GetInfoFromNode(meshNode);
modelMesh->ReplaceMesh (convexParts);
xxxx.Serialize("../../../media/xxx.ngd");
#endif
DemoEntity* const entity = new DemoEntity(NULL);
entity->SetMatrix(*scene, dQuaternion(), dVector (0, 10, 0, 0));
entity->InterpolateMatrix (*scene, 1.0f);
scene->Append (entity);
DemoMesh* const mesh1 = new DemoMesh(convexParts);
entity->SetMesh(mesh1);
mesh1->Release();
/*
DemoEntity* const entity2 = new DemoEntity(NULL);
entity2->SetMatrix(*scene, dQuaternion(), dVector (0, 10, 0, 0));
entity2->InterpolateMatrix (*scene, 1.0f);
scene->Append (entity2);
DemoMesh* const mesh2 = new DemoMesh(mesh);
entity2->SetMesh(mesh2);
mesh2->Release();
*/
// make sure the assets are released before leaving the function
if (convexParts) {
NewtonMeshDestroy (convexParts);
}
NewtonMeshDestroy (mesh);
NewtonReleaseCollision(world, collision);
}
void MakeHexapod(DemoEntityManager* const scene, const dMatrix& location)
{
dFloat mass = 30.0f;
// make the kinematic solver
m_kinematicSolver = NewtonCreateInverseDynamics(scene->GetNewton());
// make the root body
dMatrix baseMatrix(dGetIdentityMatrix());
baseMatrix.m_posit.m_y += 0.35f;
dVector size (1.3f, 0.31f, 0.5f, 0.0f);
NewtonBody* const hexaBody = CreateBox(scene, baseMatrix * location, size, mass, 1.0f);
void* const hexaBodyNode = NewtonInverseDynamicsAddRoot(m_kinematicSolver, hexaBody);
int legEffectorCount = 0;
dCustomInverseDynamicsEffector* legEffectors[32];
baseMatrix.m_posit.m_y -= 0.06f;
// make the hexapod six limbs
for (int i = 0; i < 3; i ++) {
dMatrix rightLocation (baseMatrix);
rightLocation.m_posit += rightLocation.m_right.Scale (size.m_z * 0.65f);
rightLocation.m_posit += rightLocation.m_front.Scale (size.m_x * 0.3f - size.m_x * i / 3.0f);
legEffectors[legEffectorCount] = AddLeg (scene, hexaBodyNode, rightLocation * location, mass * 0.1f, 0.3f);
legEffectorCount ++;
dMatrix similarTransform (dGetIdentityMatrix());
similarTransform.m_posit.m_x = rightLocation.m_posit.m_x;
similarTransform.m_posit.m_y = rightLocation.m_posit.m_y;
dMatrix leftLocation (rightLocation * similarTransform.Inverse() * dYawMatrix(dPi) * similarTransform);
legEffectors[legEffectorCount] = AddLeg (scene, hexaBodyNode, leftLocation * location, mass * 0.1f, 0.3f);
legEffectorCount ++;
}
// finalize inverse dynamics solver
NewtonInverseDynamicsEndBuild(m_kinematicSolver);
// create a fix pose frame generator
dEffectorTreeFixPose* const idlePose = new dEffectorTreeFixPose(hexaBody);
dEffectorTreeFixPose* const walkPoseGenerator = new dEffectorWalkPoseGenerator(hexaBody);
m_walkIdleBlender = new dEffectorBlendIdleWalk (hexaBody, idlePose, walkPoseGenerator);
m_postureModifier = new dAnimationHipController(m_walkIdleBlender);
m_animTreeNode = new dEffectorTreeRoot(hexaBody, m_postureModifier);
dMatrix rootMatrix;
NewtonBodyGetMatrix (hexaBody, &rootMatrix[0][0]);
rootMatrix = rootMatrix.Inverse();
for (int i = 0; i < legEffectorCount; i++) {
dEffectorTreeInterface::dEffectorTransform frame;
dCustomInverseDynamicsEffector* const effector = legEffectors[i];
dMatrix effectorMatrix(effector->GetBodyMatrix());
dMatrix poseMatrix(effectorMatrix * rootMatrix);
frame.m_effector = effector;
frame.m_posit = poseMatrix.m_posit;
frame.m_rotation = dQuaternion(poseMatrix);
idlePose->GetPose().Append(frame);
walkPoseGenerator->GetPose().Append(frame);
m_animTreeNode->GetPose().Append(frame);
}
}
void dCustomBallAndSocket::SubmitConstraints(dFloat timestep, int threadIndex)
{
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix(matrix0, matrix1);
SubmitLinearRows(0x07, matrix0, matrix1);
const dVector& coneDir0 = matrix0.m_front;
const dVector& coneDir1 = matrix1.m_front;
dFloat cosAngleCos = coneDir1.DotProduct3(coneDir0);
dMatrix coneRotation(dGetIdentityMatrix());
dVector lateralDir(matrix0.m_up);
if (cosAngleCos < 0.9999f) {
lateralDir = coneDir1.CrossProduct(coneDir0);
dFloat mag2 = lateralDir.DotProduct3(lateralDir);
if (mag2 > 1.0e-4f) {
lateralDir = lateralDir.Scale(1.0f / dSqrt(mag2));
coneRotation = dMatrix(dQuaternion(lateralDir, dAcos(dClamp(cosAngleCos, dFloat(-1.0f), dFloat(1.0f)))), matrix1.m_posit);
} else {
lateralDir = matrix0.m_up.Scale (-1.0f);
coneRotation = dMatrix(dQuaternion(matrix0.m_up, 180 * dDegreeToRad), matrix1.m_posit);
}
}
dVector omega0(0.0f);
dVector omega1(0.0f);
NewtonBodyGetOmega(m_body0, &omega0[0]);
if (m_body1) {
NewtonBodyGetOmega(m_body1, &omega1[0]);
}
dVector relOmega(omega0 - omega1);
// do twist angle calculations
dMatrix twistMatrix(matrix0 * (matrix1 * coneRotation).Inverse());
dFloat twistAngle = m_twistAngle.Update(dAtan2(twistMatrix[1][2], twistMatrix[1][1]));
if (m_options.m_option0) {
if ((m_minTwistAngle == 0.0f) && (m_minTwistAngle == 0.0f)) {
NewtonUserJointAddAngularRow(m_joint, -twistAngle, &matrix0.m_front[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
} else {
if (m_options.m_option1) {
// TODO spring option
dAssert (0);
} else {
SubmitConstraintTwistLimits(matrix0, matrix1, relOmega, timestep);
}
}
} else if (m_options.m_option1) {
// TODO spring option
dAssert (0);
} else if (m_twistFriction > 0.0f) {
NewtonUserJointAddAngularRow(m_joint, 0, &matrix0.m_front[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint));
NewtonUserJointSetRowMinimumFriction(m_joint, -m_twistFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_twistFriction);
}
// do twist cone angle calculations
if (m_options.m_option2) {
if ((m_maxConeAngle == 0.0f)) {
dMatrix localMatrix(matrix0 * matrix1.Inverse());
dVector euler0;
dVector euler1;
localMatrix.GetEulerAngles(euler0, euler1, m_pitchRollYaw);
NewtonUserJointAddAngularRow(m_joint, -euler0[1], &matrix1[1][0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointAddAngularRow(m_joint, -euler0[2], &matrix1[2][0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
} else {
if (m_options.m_option3) {
// TODO spring option
dAssert(0);
} else {
dFloat jointOmega = relOmega.DotProduct3(lateralDir);
dFloat currentAngle = dAcos(dClamp(cosAngleCos, dFloat(-1.0f), dFloat(1.0f)));
dFloat coneAngle = currentAngle + jointOmega * timestep;
if (coneAngle >= m_maxConeAngle) {
//dQuaternion rot(lateralDir, coneAngle);
//dVector frontDir(rot.RotateVector(coneDir1));
//dVector upDir(lateralDir.CrossProduct(frontDir));
dVector upDir(lateralDir.CrossProduct(coneDir0));
NewtonUserJointAddAngularRow(m_joint, 0.0f, &upDir[0]);
NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint));
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointAddAngularRow(m_joint, 0.0f, &lateralDir[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction);
const dFloat invtimestep = 1.0f / timestep;
const dFloat speed = 0.5f * (m_maxConeAngle - currentAngle) * invtimestep;
const dFloat stopAccel = NewtonUserJointCalculateRowZeroAccelaration(m_joint) + speed * invtimestep;
NewtonUserJointSetRowAcceleration(m_joint, stopAccel);
} else if (m_coneFriction != 0) {
NewtonUserJointAddAngularRow(m_joint, 0.0f, &lateralDir[0]);
NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint));
NewtonUserJointSetRowMinimumFriction(m_joint, -m_coneFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction);
dVector upDir(lateralDir.CrossProduct(coneDir0));
NewtonUserJointAddAngularRow(m_joint, 0.0f, &upDir[0]);
NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint));
NewtonUserJointSetRowMinimumFriction(m_joint, -m_coneFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction);
}
}
}
} else if (m_options.m_option3) {
// TODO spring option
dAssert(0);
} else if (m_coneFriction > 0.0f) {
NewtonUserJointAddAngularRow(m_joint, 0.0f, &lateralDir[0]);
NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint));
NewtonUserJointSetRowMinimumFriction(m_joint, -m_coneFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction);
dVector upDir(lateralDir.CrossProduct(coneDir0));
NewtonUserJointAddAngularRow(m_joint, 0.0f, &upDir[0]);
NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint));
NewtonUserJointSetRowMinimumFriction(m_joint, -m_coneFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction);
}
}
void dCustomBallAndSocket::Debug(dDebugDisplay* const debugDisplay) const
{
dMatrix matrix0;
dMatrix matrix1;
dCustomJoint::Debug(debugDisplay);
CalculateGlobalMatrix(matrix0, matrix1);
const dVector& coneDir0 = matrix0.m_front;
const dVector& coneDir1 = matrix1.m_front;
dFloat cosAngleCos = coneDir0.DotProduct3(coneDir1);
dMatrix coneRotation(dGetIdentityMatrix());
if (cosAngleCos < 0.9999f) {
dVector lateralDir(coneDir1.CrossProduct(coneDir0));
dFloat mag2 = lateralDir.DotProduct3(lateralDir);
//dAssert(mag2 > 1.0e-4f);
if (mag2 > 1.0e-4f) {
lateralDir = lateralDir.Scale(1.0f / dSqrt(mag2));
coneRotation = dMatrix(dQuaternion(lateralDir, dAcos(dClamp(cosAngleCos, dFloat(-1.0f), dFloat(1.0f)))), matrix1.m_posit);
} else {
lateralDir = matrix0.m_up.Scale(-1.0f);
coneRotation = dMatrix(dQuaternion(matrix0.m_up, 180 * dDegreeToRad), matrix1.m_posit);
}
} else if (cosAngleCos < -0.9999f) {
coneRotation[0][0] = -1.0f;
coneRotation[1][1] = -1.0f;
}
const int subdiv = 18;
const dFloat radius = debugDisplay->m_debugScale;
dVector arch[subdiv + 1];
// show twist angle limits
if (m_options.m_option0 && ((m_maxTwistAngle - m_minTwistAngle) > dFloat(1.0e-3f))) {
dMatrix pitchMatrix(matrix1 * coneRotation);
pitchMatrix.m_posit = matrix1.m_posit;
dVector point(dFloat(0.0f), dFloat(radius), dFloat(0.0f), dFloat(0.0f));
dFloat angleStep = dMin (m_maxTwistAngle - m_minTwistAngle, dFloat (2.0f * dPi)) / subdiv;
dFloat angle0 = m_minTwistAngle;
debugDisplay->SetColor(dVector(0.6f, 0.2f, 0.0f, 0.0f));
for (int i = 0; i <= subdiv; i++) {
arch[i] = pitchMatrix.TransformVector(dPitchMatrix(angle0).RotateVector(point));
debugDisplay->DrawLine(pitchMatrix.m_posit, arch[i]);
angle0 += angleStep;
}
for (int i = 0; i < subdiv; i++) {
debugDisplay->DrawLine(arch[i], arch[i + 1]);
}
}
// show cone angle limits
if (m_options.m_option2) {
dVector point(radius * dCos(m_maxConeAngle), radius * dSin(m_maxConeAngle), 0.0f, 0.0f);
dFloat angleStep = dPi * 2.0f / subdiv;
dFloat angle0 = 0.0f;
debugDisplay->SetColor(dVector(0.3f, 0.8f, 0.0f, 0.0f));
for (int i = 0; i <= subdiv; i++) {
dVector conePoint(dPitchMatrix(angle0).RotateVector(point));
dVector p(matrix1.TransformVector(conePoint));
arch[i] = p;
debugDisplay->DrawLine(matrix1.m_posit, p);
angle0 += angleStep;
}
for (int i = 0; i < subdiv; i++) {
debugDisplay->DrawLine(arch[i], arch[i + 1]);
}
}
}
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);
}
