void ShowJointInfo(const NewtonCustomJoint* joint)
{
NewtonJointRecord info;
if (showContacts) {
joint->GetInfo (&info);
dMatrix bodyMatrix0;
NewtonBodyGetMatrix (info.m_attachBody_0, &bodyMatrix0[0][0]);
dMatrix matrix0 (*((dMatrix*) &info.m_attachmenMatrix_0[0]));
matrix0 = matrix0 * bodyMatrix0;
DebugDrawLine (matrix0.m_posit, matrix0.m_posit + matrix0.m_front, dVector (1, 0, 0));
DebugDrawLine (matrix0.m_posit, matrix0.m_posit + matrix0.m_up, dVector (0, 1, 0));
DebugDrawLine (matrix0.m_posit, matrix0.m_posit + matrix0.m_right, dVector (0, 0, 1));
dMatrix bodyMatrix1;
NewtonBodyGetMatrix (info.m_attachBody_1, &bodyMatrix1[0][0]);
dMatrix matrix1 (*((dMatrix*) &info.m_attachmenMatrix_1[0]));
matrix1 = matrix1 * bodyMatrix1;
DebugDrawLine (matrix1.m_posit, matrix1.m_posit + matrix1.m_front, dVector (1, 0, 0));
DebugDrawLine (matrix1.m_posit, matrix1.m_posit + matrix1.m_up, dVector (0, 1, 0));
DebugDrawLine (matrix1.m_posit, matrix1.m_posit + matrix1.m_right, dVector (0, 0, 1));
}
}
void DemoSoundListener::PostUpdate (const NewtonWorld* const world, dFloat timestep)
{
DemoEntityManager* const scene = (DemoEntityManager*) NewtonWorldGetUserData(world);
DemoCamera* const camera = scene->GetCamera();
dMatrix matrix0 (camera->GetCurrentMatrix ());
dMatrix matrix1 (camera->GetNextMatrix ());
dVector veloc ((matrix1.m_posit - matrix0.m_posit).Scale (1.0f / timestep));
//printf ("%f %f %f %f\n", veloc.m_x, veloc.m_y, veloc.m_z, timestepInSecunds);
UpdateListener (matrix1.m_posit, veloc, matrix0.m_front, matrix0.m_up);
dSoundManager::Update();
}
/* Main part of macro */
void fitData( TGraphErrors *gdata , TF1* ffit , double *startval )
{
cout << "\n***** fitData.C: Starting fitting procedure... *****" << endl;
g_data_global = gdata;
f_fit_global = ffit;
if ( g_data_global == NULL )
{
cout << "fitData.C: Did no find any data to fit." << endl;
return NULL;
}
const char * minName = "Minuit2";
const char *algoName = "Migrad";
// create minimizer giving a name and a name (optionally) for the specific
// algorithm
// possible choices are:
// minName algoName
// Minuit /Minuit2 Migrad, Simplex,Combined,Scan (default is Migrad)
// Minuit2 Fumili2
// Fumili
// GSLMultiMin ConjugateFR, ConjugatePR, BFGS,
// BFGS2, SteepestDescent
// GSLMultiFit
// GSLSimAn
// Genetic
ROOT::Math::Minimizer* min =
ROOT::Math::Factory::CreateMinimizer(minName, algoName);
/* Alternatice constructor for Minuit */
//ROOT::Minuit2::Minuit2Minimizer *min = new ROOT::Minuit2::Minuit2Minimizer();
//TMinuit *min = new TMinuit();
// set tolerance , etc...
min->SetMaxFunctionCalls(1000000); // for Minuit/Minuit2
min->SetMaxIterations(10000); // for GSL
min->SetTolerance(0.001);
min->SetPrintLevel(1);
// create funciton wrapper for minmizer
// a IMultiGenFunction type
ROOT::Math::Functor f(&Chi2_log,3);
double step[3] = {0.1,0.1,10};
// starting point
double variable[3] = { startval[0],startval[1],startval[2]};
// set function to minimize (this is the Chi2 function)
min->SetFunction(f);
// Set the free variables to be minimized!
min->SetVariable(0,"x",variable[0], step[0]);
min->SetVariable(1,"y",variable[1], step[1]);
min->SetVariable(2,"z",variable[2], step[2]);
// do the minimization
min->Minimize();
/* set function parameters to best fit results */
const double *xs = min->X();
f_fit_global->SetParameter(0, xs[0]);
f_fit_global->SetParameter(1, xs[1]);
f_fit_global->SetParameter(2, xs[2]);
/* Print summaries of minimization process */
std::cout << "Minimum: f(" << xs[0] << ", " << xs[1] << ", " << xs[2] << "): "
<< min->MinValue() << std::endl;
// expected minimum is 0
if ( min->MinValue() < 1.E-4 && f(xs) < 1.E-4)
std::cout << "Minimizer " << minName << " - " << algoName
<< " converged to the right minimum" << std::endl;
else {
std::cout << "Minimizer " << minName << " - " << algoName
<< " failed to converge !!!" << std::endl;
Error("NumericalMinimization","fail to converge");
}
/* Get degrees of freedom NDF */
unsigned ndf = 0;
/* get range of function used for fit */
double fit_min, fit_max;
f_fit_global->GetRange(fit_min,fit_max);
/* count data points in range of function */
for ( int p = 0; p < g_data_global->GetN(); p++ )
{
if ( g_data_global->GetX()[p] >= fit_min && g_data_global->GetX()[p] <= fit_max )
{
ndf++;
}
}
ndf -= 3; // 3 parameters in fit
cout << "Degrees of freedom: " << ndf << " --> chi2 / NDF = " << min->MinValue() / ndf << endl;
/* Get error covariance matrix from minimizer */
TMatrixD matrix0(3,3);
for ( unsigned i = 0; i < 3; i++ )
{
for ( unsigned j = 0; j < 3; j++ )
{
matrix0[i][j] = min->CovMatrix(i,j);
}
}
matrix0.Print();
/* Get minos error */
double par0_minos_error_up = 0;
double par0_minos_error_low = 0;
double par1_minos_error_up = 0;
double par1_minos_error_low = 0;
double par2_minos_error_up = 0;
double par2_minos_error_low = 0;
min->GetMinosError( 0, par0_minos_error_low, par0_minos_error_up );
min->GetMinosError( 1, par1_minos_error_low, par1_minos_error_up );
min->GetMinosError( 2, par2_minos_error_low, par2_minos_error_up );
cout << "Minos uncertainty parameter 0: up = " << par0_minos_error_up << ", low = " << par0_minos_error_low << endl;
cout << "Minos uncertainty parameter 1: up = " << par1_minos_error_up << ", low = " << par1_minos_error_low << endl;
cout << "Minos uncertainty parameter 2: up = " << par2_minos_error_up << ", low = " << par2_minos_error_low << endl;
/* Plot chi2 */
/* sigmas symmetric from covariance matrix */
//double sigmas[6] = {sqrt(matrix0[0][0]), sqrt(matrix0[0][0]), sqrt(matrix0[1][1]), sqrt(matrix0[1][1]), sqrt(matrix0[2][2]), sqrt(matrix0[2][2])};
/* sigmas asymmetric from minos */
double sigmas[6] = {par0_minos_error_low, par0_minos_error_up, par1_minos_error_low, par1_minos_error_up, par2_minos_error_low, par2_minos_error_up};
plotChi2( g_data_global, f_fit_global, 3, sigmas );
/* Draw contour plots */
TGraph2D* gcontour3D = plotContour( min , 3 , ndf );
/* Find error boundaries */
TF1* flow = (TF1*)f_fit_global->Clone("flow");
TF1* fup = (TF1*)f_fit_global->Clone("fup");
flow->SetRange(0,100000);
flow->SetLineColor(kBlue);
flow->SetLineStyle(2);
fup->SetRange(0,100000);
fup->SetLineColor(kBlue);
fup->SetLineStyle(2);
findErrorBounds( f_fit_global, flow, fup, gcontour3D );
/* plot residuals data w.r.t. fit */
plotResiduals( g_data_global, f_fit_global, flow, fup );
/* Draw data, best fit, and 1-sigma band */
TCanvas *cfit = new TCanvas();
g_data_global->Draw("AP");
//f_fit_global->SetRange(0,4000);
f_fit_global->Draw("same");
flow->Draw("same");
fup->Draw("same");
cfit->Print("new_FitResult.png");
/* Fit Extrapolation */
double t_extrapolate_months = 6;
double t_extrapolate_s = t_extrapolate_months * 30 * 24 * 60 * 60;
cout << "Extrapolation to " << t_extrapolate_months
<< " months: " << ffit->Eval( t_extrapolate_s )
<< " , up: " << fup->Eval( t_extrapolate_s ) - ffit->Eval( t_extrapolate_s )
<< " , low: " << flow->Eval( t_extrapolate_s ) - ffit->Eval( t_extrapolate_s )
<< endl;
return f_fit_global;
}
void CreateRagdollExperiment_0(const dMatrix& location)
{
static dJointDefinition jointsDefinition[] =
{
{ "body" },
{ "leg", 100.0f,{ -15.0f, 15.0f, 30.0f },{ 0.0f, 0.0f, 180.0f }, dAnimationRagdollJoint::m_threeDof },
{ "foot", 100.0f,{ -15.0f, 15.0f, 30.0f },{ 0.0f, 90.0f, 0.0f }, dAnimationRagdollJoint::m_twoDof },
};
const int definitionCount = sizeof (jointsDefinition)/sizeof (jointsDefinition[0]);
NewtonWorld* const world = GetWorld();
DemoEntityManager* const scene = (DemoEntityManager*)NewtonWorldGetUserData(world);
DemoEntity* const modelEntity = DemoEntity::LoadNGD_mesh("selfbalance_01.ngd", GetWorld(), scene->GetShaderCache());
dMatrix matrix0(modelEntity->GetCurrentMatrix());
//matrix0.m_posit = location;
//modelEntity->ResetMatrix(*scene, matrix0);
scene->Append(modelEntity);
// add the root childBody
NewtonBody* const rootBody = CreateBodyPart(modelEntity);
// build the rag doll with rigid bodies connected by joints
dDynamicsRagdoll* const dynamicRagdoll = new dDynamicsRagdoll(rootBody, dGetIdentityMatrix());
AddModel(dynamicRagdoll);
dynamicRagdoll->SetCalculateLocalTransforms(true);
// save the controller as the collision user data, for collision culling
NewtonCollisionSetUserData(NewtonBodyGetCollision(rootBody), dynamicRagdoll);
int stackIndex = 0;
DemoEntity* childEntities[32];
dAnimationJoint* parentBones[32];
for (DemoEntity* child = modelEntity->GetChild(); child; child = child->GetSibling()) {
parentBones[stackIndex] = dynamicRagdoll;
childEntities[stackIndex] = child;
stackIndex++;
}
// walk model hierarchic adding all children designed as rigid body bones.
while (stackIndex) {
stackIndex--;
DemoEntity* const entity = childEntities[stackIndex];
dAnimationJoint* parentNode = parentBones[stackIndex];
const char* const name = entity->GetName().GetStr();
for (int i = 0; i < definitionCount; i++) {
if (!strcmp(jointsDefinition[i].m_boneName, name)) {
NewtonBody* const childBody = CreateBodyPart(entity);
// connect this body part to its parent with a rag doll joint
parentNode = CreateChildNode(childBody, parentNode, jointsDefinition[i]);
NewtonCollisionSetUserData(NewtonBodyGetCollision(childBody), parentNode);
break;
}
}
for (DemoEntity* child = entity->GetChild(); child; child = child->GetSibling()) {
parentBones[stackIndex] = parentNode;
childEntities[stackIndex] = child;
stackIndex++;
}
}
SetModelMass (100.0f, dynamicRagdoll);
// set the collision mask
// note this container work best with a material call back for setting bit field
//dAssert(0);
//controller->SetDefaultSelfCollisionMask();
// transform the entire contraction to its location
dMatrix worldMatrix(modelEntity->GetCurrentMatrix() * location);
worldMatrix.m_posit = location.m_posit;
NewtonBodySetMatrixRecursive(rootBody, &worldMatrix[0][0]);
// attach effectors here
for (dAnimationJoint* joint = GetFirstJoint(dynamicRagdoll); joint; joint = GetNextJoint(joint)) {
if (joint->GetAsRoot()) {
dAssert(dynamicRagdoll == joint);
dynamicRagdoll->SetHipEffector(joint);
} else if (joint->GetAsLeaf()) {
dynamicRagdoll->SetFootEffector(joint);
}
}
dynamicRagdoll->Finalize();
//return controller;
}
void dCustomKinematicController::SubmitConstraints (dFloat timestep, int threadIndex)
{
// check if this is an impulsive time step
dMatrix matrix0(GetBodyMatrix());
dVector omega(0.0f);
dVector com(0.0f);
dVector pointVeloc(0.0f);
const dFloat damp = 0.3f;
dAssert (timestep > 0.0f);
const dFloat invTimestep = 1.0f / timestep;
// we not longer cap excessive angular velocities, it is left to the client application.
NewtonBodyGetOmega(m_body0, &omega[0]);
//cap excessive angular velocities
dFloat mag2 = omega.DotProduct3(omega);
if (mag2 > (m_omegaCap * m_omegaCap)) {
omega = omega.Normalize().Scale(m_omegaCap);
NewtonBodySetOmega(m_body0, &omega[0]);
}
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
dVector relPosit(m_targetMatrix.m_posit - matrix0.m_posit);
NewtonBodyGetPointVelocity(m_body0, &m_targetMatrix.m_posit[0], &pointVeloc[0]);
for (int i = 0; i < 3; i ++) {
// Restrict the movement on the pivot point along all tree orthonormal direction
dFloat speed = pointVeloc.DotProduct3(m_targetMatrix[i]);
dFloat dist = relPosit.DotProduct3(m_targetMatrix[i]) * damp;
dFloat relSpeed = dist * invTimestep - speed;
dFloat relAccel = relSpeed * invTimestep;
NewtonUserJointAddLinearRow(m_joint, &matrix0.m_posit[0], &matrix0.m_posit[0], &m_targetMatrix[i][0]);
NewtonUserJointSetRowAcceleration(m_joint, relAccel);
NewtonUserJointSetRowMinimumFriction(m_joint, -m_maxLinearFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_maxLinearFriction);
}
if (m_isSixdof) {
dQuaternion rotation (matrix0.Inverse() * m_targetMatrix);
if (dAbs (rotation.m_q0) < 0.99998f) {
dMatrix rot (dGrammSchmidt(dVector (rotation.m_q1, rotation.m_q2, rotation.m_q3)));
dFloat angle = 2.0f * dAcos(dClamp(rotation.m_q0, dFloat(-1.0f), dFloat(1.0f)));
NewtonUserJointAddAngularRow (m_joint, angle, &rot.m_front[0]);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &rot.m_up[0]);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &rot.m_right[0]);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction);
} else {
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_up[0]);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_right[0]);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction);
}
}
}
