static void BuildJenga (DemoEntityManager* const scene, dFloat mass, const dVector& origin, const dVector& size, int count)
{
// build a standard block stack of 20 * 3 boxes for a total of 60
NewtonWorld* const world = scene->GetNewton();
dVector blockBoxSize (0.8f, 0.5f, 0.8f * 3.0f);
blockBoxSize = blockBoxSize.Scale (1.0f);
// create the stack
dMatrix baseMatrix (dGetIdentityMatrix());
// for the elevation of the floor at the stack position
baseMatrix.m_posit.m_x = origin.m_x;
baseMatrix.m_posit.m_z = origin.m_z;
dFloat startElevation = 100.0f;
dVector floor (FindFloor (world, dVector (baseMatrix.m_posit.m_x, startElevation, baseMatrix.m_posit.m_z, 0.0f), 2.0f * startElevation));
baseMatrix.m_posit.m_y = floor.m_y + blockBoxSize.m_y / 2.0f;
// set realistic mass and inertia matrix for each block
mass = 5.0f;
// create a 90 degree rotation matrix
dMatrix rotMatrix (dYawMatrix (3.141592f * 0.5f));
// create a material to control collision with this objects
int defaultMaterialID;
defaultMaterialID = NewtonMaterialGetDefaultGroupID (world);
// separate a bit the block alone the horizontal direction
dFloat gap = 0.01f;
// create the shape and visual mesh as a common data to be re used
NewtonCollision* const collision = CreateConvexCollision (world, dGetIdentityMatrix(), blockBoxSize, _BOX_PRIMITIVE, defaultMaterialID);
DemoMesh* const geometry = new DemoMesh("box", collision, "wood_0.tga", "wood_0.tga", "wood_0.tga");
for (int i = 0; i < count; i ++) {
dMatrix matrix(baseMatrix);
matrix.m_posit -= matrix.m_front.Scale (blockBoxSize.m_x - gap);
for (int j = 0; j < 3; j ++) {
CreateSimpleSolid (scene, geometry, mass, matrix, collision, defaultMaterialID);
matrix.m_posit += matrix.m_front.Scale (blockBoxSize.m_x + gap);
}
baseMatrix = rotMatrix * baseMatrix;
baseMatrix.m_posit += matrix.m_up.Scale (blockBoxSize.m_y * 0.99f);
}
// do not forget to release the assets
geometry->Release();
NewtonDestroyCollision (collision);
}
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;
}
void Custom6DOF::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);
// add the linear limits
const dVector& p0 = matrix0.m_posit;
const dVector& p1 = matrix1.m_posit;
dVector dp (p0 - p1);
for (int i = 0; i < 3; i ++) {
if ((m_minLinearLimits[i] == 0.0f) && (m_maxLinearLimits[i] == 0.0f)) {
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0[i][0]);
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
} else {
// it is a limited linear dof, check if it pass the limits
dFloat dist = dp.DotProduct3(matrix1[i]);
if (dist > m_maxLinearLimits[i]) {
dVector q1 (p1 + matrix1[i].Scale (m_maxLinearLimits[i]));
// clamp the error, so the not too much energy is added when constraint violation occurs
dFloat maxDist = (p0 - q1).DotProduct3(matrix1[i]);
if (maxDist > D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION) {
q1 = p0 - matrix1[i].Scale(D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION);
}
NewtonUserJointAddLinearRow (m_joint, &p0[0], &q1[0], &matrix0[i][0]);
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the object to return but not to kick going forward
NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);
} else if (dist < m_minLinearLimits[i]) {
dVector q1 (p1 + matrix1[i].Scale (m_minLinearLimits[i]));
// clamp the error, so the not too much energy is added when constraint violation occurs
dFloat maxDist = (p0 - q1).DotProduct3(matrix1[i]);
if (maxDist < -D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION) {
q1 = p0 - matrix1[i].Scale(-D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION);
}
NewtonUserJointAddLinearRow (m_joint, &p0[0], &q1[0], &matrix0[i][0]);
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the object to return but not to kick going forward
NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
}
}
}
dVector euler0(0.0f);
dVector euler1(0.0f);
dMatrix localMatrix (matrix0 * matrix1.Inverse());
localMatrix.GetEulerAngles(euler0, euler1);
AngularIntegration pitchStep0 (AngularIntegration (euler0.m_x) - m_pitch);
AngularIntegration pitchStep1 (AngularIntegration (euler1.m_x) - m_pitch);
if (dAbs (pitchStep0.GetAngle()) > dAbs (pitchStep1.GetAngle())) {
euler0 = euler1;
}
dVector euler (m_pitch.Update (euler0.m_x), m_yaw.Update (euler0.m_y), m_roll.Update (euler0.m_z), 0.0f);
//dTrace (("(%f %f %f) (%f %f %f)\n", m_pitch.m_angle * 180.0f / 3.141592f, m_yaw.m_angle * 180.0f / 3.141592f, m_roll.m_angle * 180.0f / 3.141592f, euler0.m_x * 180.0f / 3.141592f, euler0.m_y * 180.0f / 3.141592f, euler0.m_z * 180.0f / 3.141592f));
bool limitViolation = false;
for (int i = 0; i < 3; i ++) {
if (euler[i] < m_minAngularLimits[i]) {
limitViolation = true;
euler[i] = m_minAngularLimits[i];
} else if (euler[i] > m_maxAngularLimits[i]) {
limitViolation = true;
euler[i] = m_maxAngularLimits[i];
}
}
if (limitViolation) {
//dMatrix pyr (dPitchMatrix(m_pitch.m_angle) * dYawMatrix(m_yaw.m_angle) * dRollMatrix(m_roll.m_angle));
dMatrix p0y0r0 (dPitchMatrix(euler[0]) * dYawMatrix(euler[1]) * dRollMatrix(euler[2]));
dMatrix baseMatrix (p0y0r0 * matrix1);
dMatrix rotation (matrix0.Inverse() * baseMatrix);
dQuaternion quat (rotation);
if (quat.m_q0 > dFloat (0.99995f)) {
//dVector p0 (matrix0[3] + baseMatrix[1].Scale (MIN_JOINT_PIN_LENGTH));
//dVector p1 (matrix0[3] + baseMatrix[1].Scale (MIN_JOINT_PIN_LENGTH));
//NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &baseMatrix[2][0]);
//NewtonUserJointSetRowMinimumFriction(m_joint, 0.0f);
//dVector q0 (matrix0[3] + baseMatrix[0].Scale (MIN_JOINT_PIN_LENGTH));
//NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &baseMatrix[1][0]);
//NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &baseMatrix[2][0]);
} else {
dMatrix basis (dGrammSchmidt (dVector (quat.m_q1, quat.m_q2, quat.m_q3, 0.0f)));
dVector q0 (matrix0[3] + basis[1].Scale (MIN_JOINT_PIN_LENGTH));
dVector q1 (matrix0[3] + rotation.RotateVector(basis[1].Scale (MIN_JOINT_PIN_LENGTH)));
NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &basis[2][0]);
NewtonUserJointSetRowMinimumFriction(m_joint, 0.0f);
//dVector q0 (matrix0[3] + basis[0].Scale (MIN_JOINT_PIN_LENGTH));
//NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &basis[1][0]);
//NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &basis[2][0]);
}
}
}
void CustomControlledBallAndSocket::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);
// Restrict the movement on the pivot point along all tree orthonormal direction
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_front[0]);
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_up[0]);
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_right[0]);
#if 0
dVector euler0;
dVector euler1;
dMatrix localMatrix (matrix0 * matrix1.Inverse());
localMatrix.GetEulerAngles(euler0, euler1);
AngularIntegration pitchStep0 (AngularIntegration (euler0.m_x) - m_pitch);
AngularIntegration pitchStep1 (AngularIntegration (euler1.m_x) - m_pitch);
if (dAbs (pitchStep0.GetAngle()) > dAbs (pitchStep1.GetAngle())) {
euler0 = euler1;
}
dVector euler (m_pitch.Update (euler0.m_x), m_yaw.Update (euler0.m_y), m_roll.Update (euler0.m_z), 0.0f);
for (int i = 0; i < 3; i ++) {
dFloat error = m_targetAngles[i] - euler[i];
if (dAbs (error) > (0.125f * 3.14159213f / 180.0f) ) {
dFloat angularStep = dSign(error) * m_angulaSpeed * timestep;
if (angularStep > 0.0f) {
if (angularStep > error) {
angularStep = error * 0.5f;
}
} else {
if (angularStep < error) {
angularStep = error * 0.5f;
}
}
euler[i] = euler[i] + angularStep;
}
}
dMatrix p0y0r0 (dPitchMatrix(euler[0]) * dYawMatrix(euler[1]) * dRollMatrix(euler[2]));
dMatrix baseMatrix (p0y0r0 * matrix1);
dMatrix rotation (matrix0.Inverse() * baseMatrix);
dQuaternion quat (rotation);
if (quat.m_q0 > dFloat (0.99995f)) {
dVector euler0;
dVector euler1;
rotation.GetEulerAngles(euler0, euler1);
NewtonUserJointAddAngularRow(m_joint, euler0[0], &rotation[0][0]);
NewtonUserJointAddAngularRow(m_joint, euler0[1], &rotation[1][0]);
NewtonUserJointAddAngularRow(m_joint, euler0[2], &rotation[2][0]);
} else {
dMatrix basis (dGrammSchmidt (dVector (quat.m_q1, quat.m_q2, quat.m_q3, 0.0f)));
NewtonUserJointAddAngularRow (m_joint, 2.0f * dAcos (quat.m_q0), &basis[0][0]);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis[1][0]);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis[2][0]);
}
#else
matrix1 = m_targetRotation * matrix1;
dQuaternion localRotation(matrix1 * matrix0.Inverse());
if (localRotation.DotProduct(m_targetRotation) < 0.0f) {
localRotation.Scale(-1.0f);
}
dFloat angle = 2.0f * dAcos(localRotation.m_q0);
dFloat angleStep = m_angulaSpeed * timestep;
if (angleStep < angle) {
dVector axis(dVector(localRotation.m_q1, localRotation.m_q2, localRotation.m_q3, 0.0f));
axis = axis.Scale(1.0f / dSqrt(axis % axis));
// localRotation = dQuaternion(axis, angleStep);
}
dVector axis (matrix1.m_front * matrix1.m_front);
dVector axis1 (matrix1.m_front * matrix1.m_front);
//dFloat sinAngle;
//dFloat cosAngle;
//CalculatePitchAngle (matrix0, matrix1, sinAngle, cosAngle);
//float xxxx = dAtan2(sinAngle, cosAngle);
//float xxxx1 = dAtan2(sinAngle, cosAngle);
dQuaternion quat(localRotation);
if (quat.m_q0 > dFloat(0.99995f)) {
// dAssert (0);
/*
dVector euler0;
dVector euler1;
rotation.GetEulerAngles(euler0, euler1);
NewtonUserJointAddAngularRow(m_joint, euler0[0], &rotation[0][0]);
NewtonUserJointAddAngularRow(m_joint, euler0[1], &rotation[1][0]);
NewtonUserJointAddAngularRow(m_joint, euler0[2], &rotation[2][0]);
*/
} else {
dMatrix basis(dGrammSchmidt(dVector(quat.m_q1, quat.m_q2, quat.m_q3, 0.0f)));
NewtonUserJointAddAngularRow(m_joint, -2.0f * dAcos(quat.m_q0), &basis[0][0]);
NewtonUserJointAddAngularRow(m_joint, 0.0f, &basis[1][0]);
NewtonUserJointAddAngularRow(m_joint, 0.0f, &basis[2][0]);
}
#endif
}
//static void UnstableStruture (SceneManager& system, dVector location, int high)
static void BreakableStruture (SceneManager& system, dVector location, int high)
{
dFloat plankMass;
dFloat columnMass;
// /////////////////////////////////////////////////////////////////////
//
// Build a parking lot type structure
dVector columnBoxSize (3.0f, 1.0f, 1.0f);
// dVector columnBoxSize (3.0f, 3.0f, 1.0f);
dVector plankBoxSize (6.0f, 1.0f, 6.0f);
// create the stack
dMatrix baseMatrix (GetIdentityMatrix());
// get the floor position in from o the camera
baseMatrix.m_posit = location;
baseMatrix.m_posit.m_y += columnBoxSize.m_x;
// set realistic (extremes in this case for 24 bits precision) masses for the different components
// note how the newton engine handle different masses ratios without compromising stability,
// we recommend the application keep this ration under 100 for contacts and 50 for joints
columnMass = 1.0f;
plankMass = 20.0f;
// plankMass = 1.0f;
// create a material carrier to to cou collision wit ethsi obejted
int defaultMaterialID;
defaultMaterialID = NewtonMaterialGetDefaultGroupID (system.m_world);
dMatrix columAlignment (dRollMatrix(3.1416f * 0.5f));
for (int i = 0; i < high; i ++) {
NewtonBody* body;
RenderPrimitive* primitive;
dMatrix matrix(columAlignment * baseMatrix);
// add the 4 column
matrix.m_posit.m_x -= (columnBoxSize.m_z - plankBoxSize.m_x) * 0.5f;
matrix.m_posit.m_z -= (columnBoxSize.m_z - plankBoxSize.m_z) * 0.5f;
body = CreateGenericSolid (system.m_world, &system, columnMass, matrix, columnBoxSize, _RANDOM_CONVEX_HULL_PRIMITIVE, defaultMaterialID);
primitive = (RenderPrimitive*) NewtonBodyGetUserData (body);
SetBreakValue (body, 50.0f);
ConvexCastPlacement (body);
matrix.m_posit.m_x += columnBoxSize.m_z - plankBoxSize.m_x;
body = CreateGenericSolid (system.m_world, &system, columnMass, matrix, columnBoxSize, _RANDOM_CONVEX_HULL_PRIMITIVE, defaultMaterialID);
primitive = (RenderPrimitive*) NewtonBodyGetUserData (body);
SetBreakValue (body, 30.0f);
ConvexCastPlacement (body);
matrix.m_posit.m_z += columnBoxSize.m_z - plankBoxSize.m_z;
body = CreateGenericSolid (system.m_world, &system, columnMass, matrix, columnBoxSize, _RANDOM_CONVEX_HULL_PRIMITIVE, defaultMaterialID);
primitive = (RenderPrimitive*) NewtonBodyGetUserData (body);
SetBreakValue (body, 30.0f);
ConvexCastPlacement (body);
matrix.m_posit.m_x -= columnBoxSize.m_z - plankBoxSize.m_x;
body = CreateGenericSolid (system.m_world, &system, columnMass, matrix, columnBoxSize, _RANDOM_CONVEX_HULL_PRIMITIVE, defaultMaterialID);
primitive = (RenderPrimitive*) NewtonBodyGetUserData (body);
SetBreakValue (body, 30.0f);
ConvexCastPlacement (body);
// add a plank
dVector size (plankBoxSize);
size.m_x *= 0.85f;
size.m_z *= 0.85f;
body = CreateGenericSolid (system.m_world, &system, plankMass, baseMatrix, size, _BOX_PRIMITIVE, defaultMaterialID);
primitive = (RenderPrimitive*) NewtonBodyGetUserData (body);
SetBreakValue (body, 1.0f);
ConvexCastPlacement (body);
// set up for another level
baseMatrix.m_posit.m_y += (columnBoxSize.m_x + plankBoxSize.m_y);
}
dFloat mass;
NewtonBody* body;
RenderPrimitive* primitive;
PrimitiveType type = _BOX_PRIMITIVE;
dVector size (1.0f, 2.0f, 1.0f);
dMatrix matrix (GetIdentityMatrix());
mass = 10.0f;
matrix.m_posit = location;
matrix.m_posit.m_y = FindFloor (system.m_world, matrix.m_posit.m_x, matrix.m_posit.m_z) + baseMatrix.m_posit.m_y + 35.0f;
body = CreateGenericSolid (system.m_world, &system, mass, matrix, size, type, defaultMaterialID);
primitive = (RenderPrimitive*) NewtonBodyGetUserData (body);
}
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);
}
}
