void PostUpdate(dFloat timestep, int threadIndex)
{
int count = 1;
void* nodes[32];
dMatrix parentMatrix[32];
nodes[0] = NewtonInverseDynamicsGetRoot(m_kinematicSolver);
parentMatrix[0] = dGetIdentityMatrix();
NewtonWorld* const world = GetManager()->GetWorld();
DemoEntityManager* const scene = (DemoEntityManager*)NewtonWorldGetUserData(world);
while (count) {
dMatrix matrix;
count --;
void* const rootNode = nodes[count];
NewtonBody* const body = NewtonInverseDynamicsGetBody(m_kinematicSolver, rootNode);
NewtonBodyGetMatrix (body, &matrix[0][0]);
dMatrix localMatrix (matrix * parentMatrix[count]);
DemoEntity* const ent = (DemoEntity*)NewtonBodyGetUserData(body);
dQuaternion rot(localMatrix);
ent->SetMatrix(*scene, rot, localMatrix.m_posit);
matrix = matrix.Inverse();
for (void* node = NewtonInverseDynamicsGetFirstChildNode(m_kinematicSolver, rootNode); node; node = NewtonInverseDynamicsGetNextChildNode(m_kinematicSolver, node)) {
nodes[count] = node;
parentMatrix[count] = matrix;
count ++;
}
}
}
dCustomInverseDynamicsEffector* AddLeg(DemoEntityManager* const scene, void* const rootNode, const dMatrix& matrix, dFloat partMass, dFloat limbLenght)
{
NewtonBody* const parent = NewtonInverseDynamicsGetBody(m_kinematicSolver, rootNode);
dFloat inertiaScale = 4.0f;
// make limb base
dMatrix baseMatrix(dRollMatrix(dPi * 0.5f));
dMatrix cylinderMatrix(baseMatrix * matrix);
NewtonBody* const base = CreateCylinder(scene, cylinderMatrix, partMass, inertiaScale, 0.2f, 0.1f);
dCustomInverseDynamics* const baseHinge = new dCustomInverseDynamics(cylinderMatrix, base, parent);
baseHinge->SetJointTorque(1000.0f);
baseHinge->SetTwistAngle(-0.5f * dPi, 0.5f * dPi);
void* const baseHingeNode = NewtonInverseDynamicsAddChildNode(m_kinematicSolver, rootNode, baseHinge->GetJoint());
//make limb forward arm
dMatrix forwardArmMatrix(dPitchMatrix(-30.0f * dDegreeToRad));
dVector forwardArmSize(limbLenght * 0.25f, limbLenght * 0.25f, limbLenght, 0.0f);
forwardArmMatrix.m_posit += forwardArmMatrix.m_right.Scale(forwardArmSize.m_z * 0.5f);
NewtonBody* const forwardArm = CreateBox(scene, forwardArmMatrix * matrix, forwardArmSize, partMass, inertiaScale);
dMatrix forwardArmPivot(forwardArmMatrix);
forwardArmPivot.m_posit -= forwardArmMatrix.m_right.Scale(forwardArmSize.m_z * 0.5f);
dCustomInverseDynamics* const forwardArmHinge = new dCustomInverseDynamics(forwardArmPivot * matrix, forwardArm, base);
forwardArmHinge->SetJointTorque(1000.0f);
forwardArmHinge->SetTwistAngle(-0.5f * dPi, 0.5f * dPi);
void* const forwardArmHingeNode = NewtonInverseDynamicsAddChildNode(m_kinematicSolver, baseHingeNode, forwardArmHinge->GetJoint());
//make limb forward arm
dMatrix armMatrix(dPitchMatrix(-90.0f * dDegreeToRad));
dFloat armSize = limbLenght * 1.25f;
armMatrix.m_posit += forwardArmMatrix.m_right.Scale(limbLenght);
armMatrix.m_posit.m_y -= armSize * 0.5f;
NewtonBody* const arm = CreateCapsule(scene, armMatrix * matrix, partMass, inertiaScale, armSize * 0.2f, armSize);
dMatrix armPivot(armMatrix);
armPivot.m_posit.m_y += armSize * 0.5f;
dCustomInverseDynamics* const armHinge = new dCustomInverseDynamics(armPivot * matrix, arm, forwardArm);
armHinge->SetJointTorque(1000.0f);
armHinge->SetTwistAngle(-0.5f * dPi, 0.5f * dPi);
void* const armHingeNode = NewtonInverseDynamicsAddChildNode(m_kinematicSolver, forwardArmHingeNode, armHinge->GetJoint());
dMatrix effectorMatrix(dGetIdentityMatrix());
effectorMatrix.m_posit = armPivot.m_posit;
effectorMatrix.m_posit.m_y -= armSize;
dHexapodEffector* const effector = new dHexapodEffector(m_kinematicSolver, armHingeNode, parent, effectorMatrix * matrix);
effector->SetAsThreedof();
effector->SetMaxLinearFriction(partMass * DEMO_GRAVITY * 10.0f);
effector->SetMaxAngularFriction(partMass * DEMO_GRAVITY * 10.0f);
return effector;
}
dCustomJoint::dCustomJoint(NewtonInverseDynamics* const invDynSolver, void* const invDynNode)
:dCustomAlloc()
,m_localMatrix0(dGetIdentityMatrix())
,m_localMatrix1(dGetIdentityMatrix())
{
m_joint = NULL;
m_body1 = NULL;
m_maxDof = 6;
m_autoDestroy = 0;
m_stiffness = 1.0f;
m_body0 = NewtonInverseDynamicsGetBody (invDynSolver, invDynNode);
m_world = NewtonBodyGetWorld(m_body0);
m_joint = NewtonInverseDynamicsCreateEffector(invDynSolver, invDynNode, SubmitConstraints);
NewtonJointSetUserData(m_joint, this);
NewtonJointSetDestructor(m_joint, Destructor);
m_userData = NULL;
m_userDestructor = NULL;
}
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);
}
