void
ddi_get_workingcomm_(int_f77 *comm_id)
{
*comm_id = gv(ddi_working_comm);
}
void dgMatrix::EigenVectors (dgVector &eigenValues, const dgMatrix* const initialGuess)
{
dgMatrix& mat = *this;
dgMatrix eigenVectors (dgGetIdentityMatrix());
if (initialGuess) {
eigenVectors = *initialGuess;
mat = eigenVectors.Transpose4X4() * mat * eigenVectors;
}
dgVector d (mat[0][0], mat[1][1], mat[2][2], dgFloat32 (0.0f));
dgVector b (d);
for (dgInt32 i = 0; i < 50; i++) {
dgFloat32 sm = dgAbsf(mat[0][1]) + dgAbsf(mat[0][2]) + dgAbsf(mat[1][2]);
if (sm < dgFloat32 (1.0e-12f)) {
// order the eigenvalue vectors
dgVector tmp (eigenVectors.m_front * eigenVectors.m_up);
if (tmp % eigenVectors.m_right < dgFloat32(0.0f)) {
dgAssert (0.0f);
eigenVectors.m_right = eigenVectors.m_right.Scale3 (-dgFloat32(1.0f));
}
break;
}
dgFloat32 thresh = dgFloat32 (0.0f);
if (i < 3) {
thresh = (dgFloat32)(0.2f / 9.0f) * sm;
}
dgVector z (dgVector::m_zero);
for (dgInt32 ip = 0; ip < 2; ip ++) {
for (dgInt32 iq = ip + 1; iq < 3; iq ++) {
dgFloat32 g = dgFloat32 (100.0f) * dgAbsf(mat[ip][iq]);
if ((i > 3) && ((dgAbsf(d[ip]) + g) == dgAbsf(d[ip])) && ((dgAbsf(d[iq]) + g) == dgAbsf(d[iq]))) {
mat[ip][iq] = dgFloat32 (0.0f);
} else if (dgAbsf(mat[ip][iq]) > thresh) {
dgFloat32 t;
dgFloat32 h = d[iq] - d[ip];
if (dgAbsf(h) + g == dgAbsf(h)) {
t = mat[ip][iq] / h;
} else {
dgFloat32 theta = dgFloat32 (0.5f) * h / mat[ip][iq];
t = dgFloat32(1.0f) / (dgAbsf(theta) + dgSqrt(dgFloat32(1.0f) + theta * theta));
if (theta < dgFloat32 (0.0f)) {
t = -t;
}
}
dgFloat32 c = dgRsqrt (dgFloat32 (1.0f) + t * t);
dgFloat32 s = t * c;
dgFloat32 tau = s / (dgFloat32(1.0f) + c);
h = t * mat[ip][iq];
z[ip] -= h;
z[iq] += h;
d[ip] -= h;
d[iq] += h;
mat[ip][iq] = dgFloat32(0.0f);
for (dgInt32 j = 0; j <= ip - 1; j ++) {
dgFloat32 g = mat[j][ip];
dgFloat32 h = mat[j][iq];
mat[j][ip] = g - s * (h + g * tau);
mat[j][iq] = h + s * (g - h * tau);
}
for (dgInt32 j = ip + 1; j <= iq - 1; j ++) {
dgFloat32 g = mat[ip][j];
dgFloat32 h = mat[j][iq];
mat[ip][j] = g - s * (h + g * tau);
mat[j][iq] = h + s * (g - h * tau);
}
for (dgInt32 j = iq + 1; j < 3; j ++) {
dgFloat32 g = mat[ip][j];
dgFloat32 h = mat[iq][j];
mat[ip][j] = g - s * (h + g * tau);
mat[iq][j] = h + s * (g - h * tau);
}
dgVector sv (s);
dgVector tauv (tau);
dgVector gv (eigenVectors[ip]);
dgVector hv (eigenVectors[iq]);
eigenVectors[ip] -= sv.CompProduct4 (hv + gv.CompProduct4(tauv));
eigenVectors[iq] += sv.CompProduct4 (gv - hv.CompProduct4(tauv));
}
}
}
b += z;
d = b;
}
eigenValues = d;
*this = eigenVectors;
}
void gen_VARIABLE ( node_t *root, int scopedepth )
{
gv(root,scopedepth);
}
ON_Mesh* ON_ControlPolygonMesh(
const ON_NurbsSurface& nurbs_surface,
bool bCleanMesh,
ON_Mesh* input_mesh
)
{
int u0 = 0;
int u1 = nurbs_surface.CVCount(0);
int v0 = 0;
int v1 = nurbs_surface.CVCount(1);
if ( 0 == nurbs_surface.m_cv || !nurbs_surface.IsValid() )
{
ON_ERROR("ON_ControlPolygonMesh - surface is not valid");
return NULL;
}
ON_SimpleArray<double> gu(u1);
ON_SimpleArray<double> gv(v1);
gu.SetCount(u1);
gv.SetCount(v1);
nurbs_surface.GetGrevilleAbcissae(0,gu.Array());
nurbs_surface.GetGrevilleAbcissae(1,gv.Array());
ON_Interval d0 = nurbs_surface.Domain(0);
ON_Interval d1 = nurbs_surface.Domain(1);
bool bPeriodic0 = nurbs_surface.IsPeriodic(0)?true:false;
bool bIsClosed0 = bPeriodic0 ? true : (nurbs_surface.IsClosed(0)?true:false);
bool bPeriodic1 = nurbs_surface.IsPeriodic(1)?true:false;
bool bIsClosed1 = bPeriodic1 ? true : (nurbs_surface.IsClosed(1)?true:false);
if ( bPeriodic0 )
{
u1 -= (nurbs_surface.Degree(0) - 1);
while ( u1 < nurbs_surface.CVCount(0) && gu[u0] < d0[0] && gu[u1-1] <= d0[1] )
{
u0++;
u1++;
}
d0.Set(gu[u0],gu[u1-1]);
}
if ( bPeriodic1 )
{
v1 -= (nurbs_surface.Degree(1) - 1);
while ( v1 < nurbs_surface.CVCount(1) && gv[v0] < d1[0] && gv[v1-1] <= d1[1] )
{
v0++;
v1++;
}
d1.Set(gv[v0],gv[v1-1]);
}
ON_Mesh* mesh = (0 == input_mesh) ? new ON_Mesh() : input_mesh;
int vertex_count = (u1-u0)*(v1-v0);
int face_count = (u1-u0-1)*(v1-v0-1);
mesh->m_V.Reserve(vertex_count);
mesh->m_N.Reserve(vertex_count);
mesh->m_T.Reserve(vertex_count);
mesh->m_F.Reserve(face_count);
ON_3dPoint V;
ON_3dVector N;
ON_2dPoint T;
int hint[2] = {0,0};
int i, j;
int k = -1;
for ( j = v0; j < v1; j++)
{
T.y = d1.NormalizedParameterAt(gv[j]);
for ( i = u0; i < u1; i++)
{
nurbs_surface.GetCV( i, j, V);
T.x = d0.NormalizedParameterAt(gu[i]);
nurbs_surface.EvNormal(gu[i],gv[j],N,0,hint);
mesh->m_V.AppendNew() = V;
mesh->m_N.AppendNew() = N;
mesh->m_T.AppendNew() = T;
if ( i > u0 && j > v0 )
{
ON_MeshFace& f = mesh->m_F.AppendNew();
f.vi[0] = k++;
f.vi[1] = k;
f.vi[2] = mesh->m_V.Count()-1;
f.vi[3] = f.vi[2]-1;
}
}
k++;
}
u1 -= u0;
v1 -= v0;
// make sure closed seams are spot on
if ( bIsClosed0 )
{
i = 0;
for ( j = 0; j < v1; j++ )
{
k = i + (u1-1);
mesh->m_V[k] = mesh->m_V[i];
if ( bPeriodic0 )
{
mesh->m_N[k] = mesh->m_N[i];
}
i = k+1;
// do NOT synch texture coordinates
}
}
if ( bIsClosed1 )
{
for ( i = 0, k = u1*(v1-1); i < u1; i++, k++ )
{
mesh->m_V[k] = mesh->m_V[i];
if ( bPeriodic1 )
{
mesh->m_N[k] = mesh->m_N[i];
}
// do NOT synch texture coordinates
}
}
// make sure singular ends are spot on
i=0;
for ( k = 0; k < 4; k++ )
{
if ( nurbs_surface.IsSingular(k) )
{
switch(k)
{
case 0: // 0 = south
i = 0;
j = 1;
k = u1;
break;
case 1: // 1 = east
i = u1-1;
j = u1;
k = u1*v1;
break;
case 2: // 2 = north
i = u1*(v1-1);
j = 1;
k = u1*v1;
break;
case 3: // 3 = west
i = 0;
j = u1;
k = u1*(v1-1)+1;
break;
}
V = mesh->m_V[i];
for ( i = i+j; i < k; i += j )
{
mesh->m_V[i] = V;
}
}
}
if ( bCleanMesh )
{
// Clean up triangles etc.
ON_3dPoint P[4];
ON_SimpleArray<int> badfi(32);
for ( i = 0; i < mesh->m_F.Count(); i++ )
{
ON_MeshFace& f = mesh->m_F[i];
P[0] = mesh->m_V[f.vi[0]];
P[1] = mesh->m_V[f.vi[1]];
P[2] = mesh->m_V[f.vi[2]];
P[3] = mesh->m_V[f.vi[3]];
if ( P[0] == P[1] )
{
f.vi[1] = f.vi[2];
f.vi[2] = f.vi[3];
P[1] = P[2];
P[2] = P[3];
}
if ( P[1] == P[2] )
{
f.vi[2] = f.vi[3];
P[2] = P[3];
}
if ( P[2] == P[3] )
{
f.vi[2] = f.vi[3];
P[2] = P[3];
}
if ( P[3] == P[0] )
{
f.vi[0] = f.vi[1];
f.vi[1] = f.vi[2];
f.vi[2] = f.vi[3];
P[0] = P[1];
P[1] = P[2];
P[2] = P[3];
}
if ( f.vi[0] == f.vi[1]
|| f.vi[1] == f.vi[2]
|| f.vi[3] == f.vi[0]
|| P[0] == P[2] || P[1] == P[3] )
{
badfi.Append(i);
}
}
if ( badfi.Count() > 0 )
{
if ( badfi.Count() == mesh->m_F.Count() )
{
if ( input_mesh )
{
mesh->Destroy();
}
else
{
delete mesh;
}
mesh = 0;
}
else
{
// remove bad faces
i = badfi[0];
j = i+1;
k = 1;
for ( j = i+1; j < mesh->m_F.Count(); j++ )
{
if ( k < badfi.Count() && j == badfi[k] )
{
k++;
}
else
{
mesh->m_F[i++] = mesh->m_F[j];
}
}
mesh->m_F.SetCount(i);
}
// 29 May 2008: Mikko, TRR 34687:
// Added crash protection. At this point mesh is NULL if it contained all bad faces.
if ( mesh)
mesh->CullUnusedVertices();
}
}
return mesh;
}
void
StageNode::WorldCallback()
{
boost::mutex::scoped_lock lock(msg_lock);
this->sim_time.fromSec(world->SimTimeNow() / 1e6);
// We're not allowed to publish clock==0, because it used as a special
// value in parts of ROS, #4027.
if(this->sim_time.sec == 0 && this->sim_time.nsec == 0)
{
ROS_DEBUG("Skipping initial simulation step, to avoid publishing clock==0");
return;
}
// TODO make this only affect one robot if necessary
if((this->base_watchdog_timeout.toSec() > 0.0) &&
((this->sim_time - this->base_last_cmd) >= this->base_watchdog_timeout))
{
for (size_t r = 0; r < this->positionmodels.size(); r++)
this->positionmodels[r]->SetSpeed(0.0, 0.0, 0.0);
}
// Get latest laser data
for (size_t r = 0; r < this->lasermodels.size(); r++)
{
const std::vector<Stg::ModelRanger::Sensor>& sensors = this->lasermodels[r]->GetSensors();
if( sensors.size() > 1 )
ROS_WARN( "ROS Stage currently supports rangers with 1 sensor only." );
// for now we access only the zeroth sensor of the ranger - good
// enough for most laser models that have a single beam origin
const Stg::ModelRanger::Sensor& s = sensors[0];
if( s.ranges.size() )
{
// Translate into ROS message format and publish
this->laserMsgs[r].angle_min = -s.fov/2.0;
this->laserMsgs[r].angle_max = +s.fov/2.0;
this->laserMsgs[r].angle_increment = s.fov/(double)(s.sample_count-1);
this->laserMsgs[r].range_min = s.range.min;
this->laserMsgs[r].range_max = s.range.max;
this->laserMsgs[r].ranges.resize(s.ranges.size());
this->laserMsgs[r].intensities.resize(s.intensities.size());
for(unsigned int i=0; i<s.ranges.size(); i++)
{
this->laserMsgs[r].ranges[i] = s.ranges[i];
this->laserMsgs[r].intensities[i] = (uint8_t)s.intensities[i];
}
this->laserMsgs[r].header.frame_id = mapName("base_laser_link", r);
this->laserMsgs[r].header.stamp = sim_time;
this->laser_pubs_[r].publish(this->laserMsgs[r]);
}
// Also publish the base->base_laser_link Tx. This could eventually move
// into being retrieved from the param server as a static Tx.
Stg::Pose lp = this->lasermodels[r]->GetPose();
tf::Quaternion laserQ;
laserQ.setRPY(0.0, 0.0, lp.a);
tf::Transform txLaser = tf::Transform(laserQ,
tf::Point(lp.x, lp.y, 0.15));
tf.sendTransform(tf::StampedTransform(txLaser, sim_time,
mapName("base_link", r),
mapName("base_laser_link", r)));
// Send the identity transform between base_footprint and base_link
tf::Transform txIdentity(tf::createIdentityQuaternion(),
tf::Point(0, 0, 0));
tf.sendTransform(tf::StampedTransform(txIdentity,
sim_time,
mapName("base_footprint", r),
mapName("base_link", r)));
// Get latest odometry data
// Translate into ROS message format and publish
this->odomMsgs[r].pose.pose.position.x = this->positionmodels[r]->est_pose.x;
this->odomMsgs[r].pose.pose.position.y = this->positionmodels[r]->est_pose.y;
this->odomMsgs[r].pose.pose.orientation = tf::createQuaternionMsgFromYaw(this->positionmodels[r]->est_pose.a);
Stg::Velocity v = this->positionmodels[r]->GetVelocity();
this->odomMsgs[r].twist.twist.linear.x = v.x;
this->odomMsgs[r].twist.twist.linear.y = v.y;
this->odomMsgs[r].twist.twist.angular.z = v.a;
//@todo Publish stall on a separate topic when one becomes available
//this->odomMsgs[r].stall = this->positionmodels[r]->Stall();
//
this->odomMsgs[r].header.frame_id = mapName("odom", r);
this->odomMsgs[r].header.stamp = sim_time;
this->odom_pubs_[r].publish(this->odomMsgs[r]);
// broadcast odometry transform
tf::Quaternion odomQ;
tf::quaternionMsgToTF(odomMsgs[r].pose.pose.orientation, odomQ);
tf::Transform txOdom(odomQ,
tf::Point(odomMsgs[r].pose.pose.position.x,
odomMsgs[r].pose.pose.position.y, 0.0));
tf.sendTransform(tf::StampedTransform(txOdom, sim_time,
mapName("odom", r),
mapName("base_footprint", r)));
// Also publish the ground truth pose and velocity
Stg::Pose gpose = this->positionmodels[r]->GetGlobalPose();
Stg::Velocity gvel = this->positionmodels[r]->GetGlobalVelocity();
// Note that we correct for Stage's screwed-up coord system.
tf::Quaternion q_gpose;
q_gpose.setRPY(0.0, 0.0, gpose.a-M_PI/2.0);
tf::Transform gt(q_gpose, tf::Point(gpose.y, -gpose.x, 0.0));
tf::Quaternion q_gvel;
q_gvel.setRPY(0.0, 0.0, gvel.a-M_PI/2.0);
tf::Transform gv(q_gvel, tf::Point(gvel.y, -gvel.x, 0.0));
this->groundTruthMsgs[r].pose.pose.position.x = gt.getOrigin().x();
this->groundTruthMsgs[r].pose.pose.position.y = gt.getOrigin().y();
this->groundTruthMsgs[r].pose.pose.position.z = gt.getOrigin().z();
this->groundTruthMsgs[r].pose.pose.orientation.x = gt.getRotation().x();
this->groundTruthMsgs[r].pose.pose.orientation.y = gt.getRotation().y();
this->groundTruthMsgs[r].pose.pose.orientation.z = gt.getRotation().z();
this->groundTruthMsgs[r].pose.pose.orientation.w = gt.getRotation().w();
this->groundTruthMsgs[r].twist.twist.linear.x = gv.getOrigin().x();
this->groundTruthMsgs[r].twist.twist.linear.y = gv.getOrigin().y();
//this->groundTruthMsgs[r].twist.twist.angular.z = tf::getYaw(gv.getRotation());
//this->groundTruthMsgs[r].twist.twist.linear.x = gvel.x;
//this->groundTruthMsgs[r].twist.twist.linear.y = gvel.y;
this->groundTruthMsgs[r].twist.twist.angular.z = gvel.a;
this->groundTruthMsgs[r].header.frame_id = mapName("odom", r);
this->groundTruthMsgs[r].header.stamp = sim_time;
this->ground_truth_pubs_[r].publish(this->groundTruthMsgs[r]);
//Publish blobfinder data
std::vector<Stg::ModelBlobfinder::Blob> blob_list = this->blobfndrmodels[r]->GetBlobs();
if ( blob_list.size() == 1 )
{
this->colorMsgs[r].r = blob_list[0].color.r;
this->colorMsgs[r].g = blob_list[0].color.g;
this->colorMsgs[r].b = blob_list[0].color.b;
this->colorMsgs[r].a = blob_list[0].color.a;
this->colorMsgs[r].blob_range = blob_list[0].range;
uint32_t left = blob_list[0].left;
uint32_t right = blob_list[0].right;
uint32_t top = blob_list[0].top;
uint32_t bottom = blob_list[0].bottom;
this->colorMsgs[r].blob_area = abs(left - right) * abs(top - bottom);
this->blob_pubs_[r].publish(this->colorMsgs[r]);
}
}
this->clockMsg.clock = sim_time;
this->clock_pub_.publish(this->clockMsg);
}
/* generate an integer binary operation */
void gen_opi(int op)
{
int r, fr, opc, c;
switch(op) {
case '+':
case TOK_ADDC1: /* add with carry generation */
opc = 0;
gen_op8:
if ((vtop->r & (VT_VALMASK | VT_LVAL | VT_SYM)) == VT_CONST) {
/* constant case */
vswap();
r = gv(RC_INT);
vswap();
c = vtop->c.i;
if (c == (char)c) {
/* XXX: generate inc and dec for smaller code ? */
o(0x83);
o(0xc0 | (opc << 3) | r);
g(c);
} else {
o(0x81);
oad(0xc0 | (opc << 3) | r, c);
}
} else {
gv2(RC_INT, RC_INT);
r = vtop[-1].r;
fr = vtop[0].r;
o((opc << 3) | 0x01);
o(0xc0 + r + fr * 8);
}
vtop--;
if (op >= TOK_ULT && op <= TOK_GT) {
vtop->r = VT_CMP;
vtop->c.i = op;
}
break;
case '-':
case TOK_SUBC1: /* sub with carry generation */
opc = 5;
goto gen_op8;
case TOK_ADDC2: /* add with carry use */
opc = 2;
goto gen_op8;
case TOK_SUBC2: /* sub with carry use */
opc = 3;
goto gen_op8;
case '&':
opc = 4;
goto gen_op8;
case '^':
opc = 6;
goto gen_op8;
case '|':
opc = 1;
goto gen_op8;
case '*':
gv2(RC_INT, RC_INT);
r = vtop[-1].r;
fr = vtop[0].r;
vtop--;
o(0xaf0f); /* imul fr, r */
o(0xc0 + fr + r * 8);
break;
case TOK_SHL:
opc = 4;
goto gen_shift;
case TOK_SHR:
opc = 5;
goto gen_shift;
case TOK_SAR:
opc = 7;
gen_shift:
opc = 0xc0 | (opc << 3);
if ((vtop->r & (VT_VALMASK | VT_LVAL | VT_SYM)) == VT_CONST) {
/* constant case */
vswap();
r = gv(RC_INT);
vswap();
c = vtop->c.i & 0x1f;
o(0xc1); /* shl/shr/sar $xxx, r */
o(opc | r);
g(c);
} else {
/* we generate the shift in ecx */
gv2(RC_INT, RC_ECX);
r = vtop[-1].r;
o(0xd3); /* shl/shr/sar %cl, r */
o(opc | r);
}
vtop--;
break;
case '/':
case TOK_UDIV:
case TOK_PDIV:
case '%':
case TOK_UMOD:
case TOK_UMULL:
/* first operand must be in eax */
/* XXX: need better constraint for second operand */
gv2(RC_EAX, RC_ECX);
r = vtop[-1].r;
fr = vtop[0].r;
vtop--;
save_reg(TREG_EDX);
if (op == TOK_UMULL) {
o(0xf7); /* mul fr */
o(0xe0 + fr);
vtop->r2 = TREG_EDX;
r = TREG_EAX;
} else {
if (op == TOK_UDIV || op == TOK_UMOD) {
o(0xf7d231); /* xor %edx, %edx, div fr, %eax */
o(0xf0 + fr);
} else {
o(0xf799); /* cltd, idiv fr, %eax */
o(0xf8 + fr);
}
if (op == '%' || op == TOK_UMOD)
r = TREG_EDX;
else
r = TREG_EAX;
}
vtop->r = r;
break;
default:
opc = 7;
goto gen_op8;
}
}
/** @see ddi_armci.h */
void DDI_ARMCI_DLBReset() {
const DDI_Comm *comm = (const DDI_Comm *) Comm_find(DDI_WORKING_COMM);
int pid = comm->me;
int armciPid = comm->global_pid[pid];
if (pid == 0) ARMCI_PutValueInt(0, gv(dlb_counter), armciPid);
}
/* convert from one floating point type to another */
ST_FUNC void gen_cvt_ftof(int t)
{
/* all we have to do on i386 is to put the float in a register */
gv(RC_FLOAT);
}
/* -------------------------------------------------------------------- *\
DDI_Timer_reset()
=================
Reset the cpu timer within the current operating scope.
\* -------------------------------------------------------------------- */
void DDI_Timer_reset() {
getrusage(RUSAGE_SELF,&gv(cpu_timer));
gettimeofday(&gv(wall_timer),NULL);
}
/**
* @return Path to the thumbnail of the given DXF file. If no thumbnail exists, one is
* created in the user's home. If no thumbnail can be created, an empty string is returned.
*/
QString QG_LibraryWidget::getPathToPixmap(const QString& dir,
const QString& dxfFile,
const QString& dxfPath) {
// the thumbnail must be created in the user's home.
#if QT_VERSION < 0x040400
QString iconCacheLocation = emu_qt44_storageLocationData() + QDir::separator() + "iconCache" + QDir::separator();
#elif QT_VERSION >= 0x050000
QString iconCacheLocation=QStandardPaths::writableLocation(QStandardPaths::DataLocation) + QDir::separator() + "iconCache" + QDir::separator();
#else
QString iconCacheLocation=QDesktopServices::storageLocation(QDesktopServices::DataLocation) + QDir::separator() + "iconCache" + QDir::separator();
#endif
RS_DEBUG->print("QG_LibraryWidget::getPathToPixmap: "
"dir: '%s' dxfFile: '%s' dxfPath: '%s'",
dir.toLatin1().data(), dxfFile.toLatin1().data(), dxfPath.toLatin1().data());
// List of all directories that contain part libraries:
QStringList directoryList = RS_SYSTEM->getDirectoryList("library");
directoryList.prepend(iconCacheLocation);
QStringList::Iterator it;
QFileInfo fiDxf(dxfPath);
QString itemDir;
QString pngPath;
// look in all possible system directories for PNG files
// in the current library path:
for (it=directoryList.begin(); it!=directoryList.end(); ++it) {
itemDir = (*it)+dir;
pngPath = itemDir + QDir::separator() + fiDxf.baseName() + ".png";
RS_DEBUG->print("QG_LibraryWidget::getPathToPixmap: checking: '%s'",
pngPath.toLatin1().data());
QFileInfo fiPng(pngPath);
// the thumbnail exists:
if (fiPng.isFile()) {
RS_DEBUG->print("QG_LibraryWidget::getPathToPixmap: dxf date: %s, png date: %s",
fiDxf.lastModified().toString().toLatin1().data(), fiPng.lastModified().toString().toLatin1().data());
if (fiPng.lastModified() > fiDxf.lastModified()) {
RS_DEBUG->print("QG_LibraryWidget::getPathToPixmap: thumbnail found: '%s'",
pngPath.toLatin1().data());
return pngPath;
} else {
RS_DEBUG->print("QG_LibraryWidget::getPathToPixmap: thumbnail needs to be updated: '%s'",
pngPath.toLatin1().data());
}
}
}
// create all directories needed:
RS_SYSTEM->createPaths(iconCacheLocation + dir);
// QString foo=iconCacheLocation + dir + QDir::separator() + fiDxf.baseName() + ".png";
pngPath = iconCacheLocation + dir + QDir::separator() + fiDxf.baseName() + ".png";
QPixmap* buffer = new QPixmap(128,128);
RS_PainterQt painter(buffer);
painter.setBackground(RS_Color(255,255,255));
painter.eraseRect(0,0, 128,128);
RS_StaticGraphicView gv(128,128, &painter);
RS_Graphic graphic;
if (graphic.open(dxfPath, RS2::FormatUnknown)) {
gv.setContainer(&graphic);
gv.zoomAuto(false);
// gv.drawEntity(&graphic, true);
for (RS_Entity* e=graphic.firstEntity(RS2::ResolveAll);
e; e=graphic.nextEntity(RS2::ResolveAll)) {
if (e->rtti() != RS2::EntityHatch){
RS_Pen pen = e->getPen();
pen.setColor(Qt::black);
e->setPen(pen);
}
gv.drawEntity(&painter, e);
}
QImageWriter iio;
QImage img;
img = buffer->toImage();
img = img.scaled(64,64, Qt::IgnoreAspectRatio, Qt::SmoothTransformation );
// iio.setImage(img);
iio.setFileName(pngPath);
iio.setFormat("PNG");
if (!iio.write(img)) {
pngPath = "";
RS_DEBUG->print(RS_Debug::D_ERROR,
"QG_LibraryWidget::getPathToPixmap: Cannot write thumbnail: '%s'",
pngPath.toLatin1().data());
}
} else {
RS_DEBUG->print(RS_Debug::D_ERROR,
"QG_LibraryWidget::getPathToPixmap: Cannot open file: '%s'",
dxfPath.toLatin1().data());
}
// GraphicView deletes painter
painter.end();
delete buffer;
return pngPath;
}
void Comm_create(int np,int *ids, int ngroups, int mygroup, int comm_id, int *new_comm_id) {
int i,ip,in,ismp,nn,nid,np_local,tmp;
size_t size;
const DDI_Comm *cur_comm = (DDI_Comm *) Comm_find(comm_id);
const DDI_Comm *comm_world = &gv(ddi_base_comm);
DDI_Comm *new_comm = (DDI_Comm *) Malloc(sizeof(DDI_Comm));
DDI_Comm *end_comm = (DDI_Comm *) Comm_find_end();
DEBUG_ROOT(LVL1,(stdout," DDI: Entering DDI_Create_comm.\n"))
DEBUG_OUT(LVL2,(stdout,"%s: Entering DDI_Create_comm.\n",DDI_Id()))
Comm_sync(123,cur_comm);
/* ------------------------------- *\
Add new_comm to the linked list
\* ------------------------------- */
new_comm->next = NULL;
end_comm->next = (void *) new_comm;
/* new_data->next = NULL; */
/* end_data->next = (void *) new_data; */
new_comm->ngroups = ngroups;
new_comm->mygroup = mygroup;
new_comm->id = *new_comm_id = gv(ddi_comm_id)++;
new_comm->local_nid = (int *) Malloc(np*sizeof(int));
new_comm->global_pid = (int *) Malloc(np*sizeof(int));
new_comm->global_nid = (int *) Malloc(np*sizeof(int));
i = 0;
if(np > 1) {
do {
if(ids[i] > cur_comm->np) {
fprintf(stdout,"%s: Invalid id list in DDI_Comm_create.\n",DDI_Id());
Fatal_error(911);
}
if(ids[i+1] < ids[i]) {
tmp = ids[i];
ids[i] = ids[i+1];
ids[i+1] = tmp;
if(i) i--;
i--;
}
} while(++i < np-1);
}
Comm_sync(126,cur_comm);
nn = -1;
nid = -1;
np_local = 0;
for(i=0; i<np; i++) {
new_comm->global_pid[i] = cur_comm->global_pid[ids[i]];
new_comm->global_nid[i] = cur_comm->global_nid[ids[i]];
fflush(stdout);
if(new_comm->global_nid[i] != nid) {
nid = new_comm->global_nid[i];
nn++;
}
new_comm->local_nid[i] = nn;
if(nid == comm_world->my) np_local++;
}
nn++;
/*
fprintf(stdout,"%s: new_comm->nn = %i.\n",DDI_Id(),nn);
fprintf(stdout,"%s: new_comm->np_local = %i.\n",DDI_Id(),np_local);
*/
Comm_sync(127,cur_comm);
DEBUG_ROOT(LVL5,(stdout," comm_create - global_pid/global_nid formed.\n"))
new_comm->smp_pid = (int *) Malloc(np_local*sizeof(int));
new_comm->node_master = (int *) Malloc(nn*sizeof(int));
new_comm->global_dsid = (int *) Malloc(nn*sizeof(int));
for(ip=0,in=-1,ismp=0,nid=-1; ip<np; ip++) {
if(new_comm->global_nid[ip] != nid) {
in++;
nid = new_comm->global_nid[ip];
new_comm->global_dsid[in] = comm_world->global_dsid[nid];
new_comm->node_master[in] = new_comm->global_pid[ip];
}
if(new_comm->global_pid[ip] == comm_world->me) {
new_comm->me = ip;
new_comm->my = in;
}
if(nid == comm_world->my) {
if(new_comm->global_pid[ip] == comm_world->me) new_comm->me_local = ismp;
new_comm->smp_pid[ismp++] = new_comm->global_pid[ip];
}
}
new_comm->nn = nn;
new_comm->np = np;
new_comm->np_local = ismp;
DEBUG_OUT(LVL5,(stdout,"%s: np=%i, nn=%i, np_smp=%i, me=%i, my=%i, me_smp=%i.\n",
DDI_Id(),new_comm->np,new_comm->nn,new_comm->np_local,
new_comm->me,new_comm->my,new_comm->me_local))
# if defined DDI_MPI
new_comm->world_comm = cur_comm->world_comm;
MPI_Comm_split(cur_comm->smp_comm,new_comm->global_pid[0],new_comm->me_local,&new_comm->smp_comm);
MPI_Comm_split(cur_comm->compute_comm,new_comm->global_pid[0],new_comm->me,&new_comm->compute_comm);
MPI_Comm_split(new_comm->compute_comm,new_comm->me_local,new_comm->my,&new_comm->node_comm);
# endif
DEBUG_OUT(LVL3,(stdout,"%s: Exiting DDI_Comm_create.\n",DDI_Id()))
}
A pString_Connection::readBurst(void)
{
ipcWarn(wrnlvl(),"%t pString_Connection::readBurst\n");
MSBuffer bbuff;
A d,z=(A)0;
I slen=readFileLength(),slen1,n,s,count;
if(-1==slen)R(A)0;
if(0==slen)
{
static char fmt[]="\343 IPC warning: pA::ReadBurst: read event with no data [%d]\n";
Warn(fmt, handle());
}
/* create buff to hold it. Fill buffer */
slen1=slen?slen:4;
bbuff.minofbuffer(mab(slen1));
bbuff.maxofbuffer(bbuff.minofbuffer()+slen1);
bbuff.reset();
if(0>(n=readTheBuffer(&bbuff,slen1))) {mfbuffer(&bbuff); R(A)0;}
if(0==n&&0==slen) {turnInReadOff(); mfbuffer(&bbuff); R(A)0;}
d=getAobjFromBuffer(&bbuff);
if((A)0==d){mfbuffer(&bbuff); R(A)0;}
// determine how many more complete A-objects lie in bbuff
count=1;
for(C *cp=bbuff.get();cp<bbuff.put();cp+=s)
{
s=longAt(cp);
cp+=sizeof(long);
if(s<=bbuff.put()-cp)++count;
}
// create result
z=gv(Et,count);
for(int i=0;i<count;++i)z->p[i]=(I)aplus_nl;
int idx=0;
z->p[idx++]=(I)d;
// retrieve additional A-objects from bbuff, fill in z
while(idx<count)
{
d=getAobjFromBuffer(&bbuff);
if((A)0==d)break;
z->p[idx++]=(I)d;
}
if(idx<count)
{
ipcWarn(wrnlvl(),"%t burst mode aborted. Possible data loss.\n");
}
// run once more to clear out bbuff and move partial object into connection
// buffers
if(bbuff.get()==bbuff.put())turnInReadOff();
else
{
d=getAobjFromBuffer(&bbuff);
if((A)0!=d || bbuff.get()!=bbuff.put())
{
ipcWarn(wrnlvl(),"%t burst buffer not cleared: %d %d %d\n",
d,bbuff.get(),bbuff.put());
}
}
// free bbuff;
mfbuffer(&bbuff);
return z;
}
/* generate a test. set 'inv' to invert test. Stack entry is popped */
int gtst(int inv, int t)
{
int v, *p, c;
v = vtop->r & VT_VALMASK;
if (v == VT_CMP) {
c = vtop->c.i ^ inv;
switch(c) {
case TOK_EQ:
c = IL_OP_BEQ;
break;
case TOK_NE:
c = IL_OP_BNE_UN;
break;
case TOK_LT:
c = IL_OP_BLT;
break;
case TOK_LE:
c = IL_OP_BLE;
break;
case TOK_GT:
c = IL_OP_BGT;
break;
case TOK_GE:
c = IL_OP_BGE;
break;
case TOK_ULT:
c = IL_OP_BLT_UN;
break;
case TOK_ULE:
c = IL_OP_BLE_UN;
break;
case TOK_UGT:
c = IL_OP_BGT_UN;
break;
case TOK_UGE:
c = IL_OP_BGE_UN;
break;
}
t = out_opj(c, t);
} else if (v == VT_JMP || v == VT_JMPI) {
/* && or || optimization */
if ((v & 1) == inv) {
/* insert vtop->c jump list in t */
p = &vtop->c.i;
while (*p != 0)
p = (int *)*p;
*p = t;
t = vtop->c.i;
} else {
t = gjmp(t);
gsym(vtop->c.i);
}
} else {
if (is_float(vtop->t)) {
vpushi(0);
gen_op(TOK_NE);
}
if ((vtop->r & (VT_VALMASK | VT_LVAL | VT_FORWARD)) == VT_CONST) {
/* constant jmp optimization */
if ((vtop->c.i != 0) != inv)
t = gjmp(t);
} else {
v = gv(RC_INT);
t = out_opj(IL_OP_BRTRUE - inv, t);
}
}
vtop--;
return t;
}
/** @see ddi_armci.h */
void DDI_ARMCI_GDLBReset() {
const DDI_Comm *comm = (const DDI_Comm *) Comm_find(DDI_COMM_WORLD);
int armciPid = comm->me;
if (armciPid == 0) ARMCI_PutValueInt(0, gv(gdlb_counter), 0);
}
void
ddi_get_working_smp_comm_(int_f77 *comm_id)
{
const DDI_Comm *comm = (DDI_Comm *) Comm_find(gv(ddi_working_comm));
*comm_id = comm->smp_comm;
}
/* -------------------------------------------------------------------- *\
DDI_Timer_output()
==================
Synchronous barrier on compute processes, but also collects total
cpu time from each compute process and prints the totals stdout.
\* -------------------------------------------------------------------- */
void DDI_Timer_output() {
int i,me,np;
struct rusage mycputime;
struct rusage *timings = NULL;
struct timeval cpu_total;
struct timeval wall_total;
DDI_NProc(&np,&me);
DDI_Sync(3081);
if(me == 0) {
timings = (struct rusage *) Malloc(np*sizeof(struct rusage));
getrusage(RUSAGE_SELF,timings);
gettimeofday(&wall_total,NULL);
} else {
getrusage(RUSAGE_SELF,&mycputime);
timings = &mycputime;
}
timings->ru_utime.tv_sec -= gv(cpu_timer).ru_utime.tv_sec;
timings->ru_utime.tv_usec -= gv(cpu_timer).ru_utime.tv_usec;
if(timings->ru_utime.tv_usec < 0) {
timings->ru_utime.tv_sec--;
timings->ru_utime.tv_usec += 1000000;
}
timings->ru_stime.tv_sec -= gv(cpu_timer).ru_stime.tv_sec;
timings->ru_stime.tv_usec -= gv(cpu_timer).ru_stime.tv_usec;
if(timings->ru_stime.tv_usec < 0) {
timings->ru_stime.tv_sec--;
timings->ru_stime.tv_usec += 1000000;
}
wall_total.tv_sec -= gv(wall_timer).tv_sec;
wall_total.tv_usec -= gv(wall_timer).tv_usec;
if(wall_total.tv_usec < 0) {
wall_total.tv_sec--;
wall_total.tv_usec += 1000000;
}
if(me == 0) {
for(i=1; i<np; i++) DDI_Recv(&timings[i],sizeof(struct rusage),i);
fprintf(stdout,"\n ------------------------------------------------");
fprintf(stdout,"\n CPU timing information for all compute processes");
fprintf(stdout,"\n ================================================");
for(i=0; i<np; i++) {
cpu_total.tv_sec = timings[i].ru_utime.tv_sec + timings[i].ru_stime.tv_sec;
cpu_total.tv_usec = timings[i].ru_utime.tv_usec + timings[i].ru_stime.tv_usec;
if(cpu_total.tv_usec > 1000000) {
cpu_total.tv_sec++;
cpu_total.tv_usec -= 1000000;
}
fprintf(stdout,"\n %4i: %d.%.6d + %d.%.6d = %d.%.6d",i,
(int)timings[i].ru_utime.tv_sec,(int)timings[i].ru_utime.tv_usec,
(int)timings[i].ru_stime.tv_sec,(int)timings[i].ru_stime.tv_usec,
(int)cpu_total.tv_sec,(int)cpu_total.tv_usec);
}
fprintf(stdout,"\n Wall: %d.%.6d",
(int) wall_total.tv_sec, (int) wall_total.tv_usec);
fprintf(stdout,"\n ================================================\n\n");
fflush(stdout);
free(timings);
} else {
DDI_Send(&mycputime,sizeof(struct rusage),0);
}
DDI_Sync(3082);
}
/* Generate function call. The function address is pushed first, then
all the parameters in call order. This functions pops all the
parameters and the function address. */
void gfunc_call(int nb_args)
{
int size, align, r, args_size, i, func_call;
Sym *func_sym;
args_size = 0;
for(i = 0;i < nb_args; i++) {
if ((vtop->type.t & VT_BTYPE) == VT_STRUCT) {
size = type_size(&vtop->type, &align);
/* align to stack align size */
size = (size + 3) & ~3;
/* allocate the necessary size on stack */
oad(0xec81, size); /* sub $xxx, %esp */
/* generate structure store */
r = get_reg(RC_INT);
o(0x89); /* mov %esp, r */
o(0xe0 + r);
vset(&vtop->type, r | VT_LVAL, 0);
vswap();
vstore();
args_size += size;
} else if (is_float(vtop->type.t)) {
gv(RC_FLOAT); /* only one float register */
if ((vtop->type.t & VT_BTYPE) == VT_FLOAT)
size = 4;
else if ((vtop->type.t & VT_BTYPE) == VT_DOUBLE)
size = 8;
else
size = 12;
oad(0xec81, size); /* sub $xxx, %esp */
if (size == 12)
o(0x7cdb);
else
o(0x5cd9 + size - 4); /* fstp[s|l] 0(%esp) */
g(0x24);
g(0x00);
args_size += size;
} else {
/* simple type (currently always same size) */
/* XXX: implicit cast ? */
r = gv(RC_INT);
if ((vtop->type.t & VT_BTYPE) == VT_LLONG) {
size = 8;
o(0x50 + vtop->r2); /* push r */
} else {
size = 4;
}
o(0x50 + r); /* push r */
args_size += size;
}
vtop--;
}
save_regs(0); /* save used temporary registers */
func_sym = vtop->type.ref;
func_call = func_sym->r;
/* fast call case */
if (func_call >= FUNC_FASTCALL1 && func_call <= FUNC_FASTCALL3) {
int fastcall_nb_regs;
fastcall_nb_regs = func_call - FUNC_FASTCALL1 + 1;
for(i = 0;i < fastcall_nb_regs; i++) {
if (args_size <= 0)
break;
o(0x58 + fastcall_regs[i]); /* pop r */
/* XXX: incorrect for struct/floats */
args_size -= 4;
}
}
gcall_or_jmp(0);
if (args_size && func_sym->r != FUNC_STDCALL)
gadd_sp(args_size);
vtop--;
}
AngleMap::AngleMap(const deg tth)
{
size_ = gSession->imageSize();
arrAngles_.resize(size_.count());
const Detector& geo = gSession->params.detector;
// compute angles:
// detector center is at vec{d} = (d_x, 0, )
// detector pixel (i,j) is at vec{b}
const double t = tth.toRad();
const double c = cos(t);
const double s = sin(t);
const double d_z = geo.detectorDistance.val();
const double b_x1 = d_z * s;
const double b_z1 = d_z * c;
int midPixX = size_.w/2 + geo.pixOffset[0].val();
int midPixY = size_.h/2 + geo.pixOffset[1].val();
for (int i=0; i<size_.w; ++i) {
const double d_x = (i - midPixX) * geo.pixSize.val();
const double b_x = b_x1 + d_x * c;
const double b_z = b_z1 - d_x * s;
const double b_x2 = b_x * b_x;
for (int j=0; j<size_.h; ++j) {
const double b_y = (midPixY - j) * geo.pixSize.val(); // == d_y
const double b_r = sqrt(b_x2 + b_y * b_y);
const rad gma = atan2(b_y, b_x);
const rad tth = atan2(b_r, b_z);
arrAngles_[pointToIndex(i, j)] = {tth.toDeg(), gma.toDeg()};
}
}
const ImageCut& cut = gSession->params.imageCut;
ASSERT(size_.w > cut.horiz());
ASSERT(size_.h > cut.vertical());
const int countAfterCut = (size_.w - cut.horiz()) * (size_.h - cut.vertical());
ASSERT(countAfterCut > 0);
// compute ranges rgeTth_, rgeGma_, rgeGmaFull_, and arrays gmas_, gmaIndexes_:
rgeTth_.invalidate();
rgeGma_.invalidate();
rgeGmaFull_.invalidate();
gmas_.resize(countAfterCut);
gmaIndexes_.resize(countAfterCut);
int gi = 0;
for (int j = cut.top.val(), jEnd = size_.h - cut.bottom.val(); j < jEnd; ++j) {
for (int i = cut.left.val(), iEnd = size_.w - cut.right.val(); i < iEnd; ++i) {
// (loop order j-i results in faster stable_sort than order i-j)
const ScatterDirection& dir = arrAngles_[pointToIndex(i, j)];
gmas_[gi] = dir.gma;
gmaIndexes_[gi] = pointToIndex(i, j);
++gi;
rgeTth_.extendBy(dir.tth);
rgeGmaFull_.extendBy(dir.gma);
// TODO URGENT: THIS IS WRONG: seems correct only for tth<=90deg
if (dir.tth >= tth)
rgeGma_.extendBy(dir.gma); // gma range at mid tth
}
}
//qDebug() << "AngleMap: found gamma range " << rgeGma_.to_s();
//qDebug() << "AngleMap: found full range " << rgeGmaFull_.to_s();
//qDebug() << "AngleMap: found theta range " << rgeTth_.to_s();
// compute indices of sorted gmas_:
std::vector<int> is(countAfterCut);
for (int i=0; i<is.size(); ++i)
is[i] = i;
//qDebug() << "AngleMap: sort";
// empirically, stable_sort seems to be faster than sort
std::stable_sort(is.begin(), is.end(), [this](int i1, int i2) {
return gmas_.at(i1) < gmas_.at(i2); });
//qDebug() << "AngleMap: sort/";
// sort gmas_:
std::vector<deg> gv(countAfterCut);
for (int i=0; i<countAfterCut; ++i)
gv[i] = gmas_.at(is.at(i));
gmas_ = std::move(gv);
// sort gmaIndexes_:
std::vector<int> uv(countAfterCut);
for (int i=0; i<countAfterCut; ++i)
uv[i] = gmaIndexes_.at(is.at(i));
gmaIndexes_ = std::move(uv);
//qDebug() << "AngleMap/";
}
/* XXX: need to use ST1 too */
void gen_opf(int op)
{
int a, ft, fc, swapped, r;
/* convert constants to memory references */
if ((vtop[-1].r & (VT_VALMASK | VT_LVAL)) == VT_CONST) {
vswap();
gv(RC_FLOAT);
vswap();
}
if ((vtop[0].r & (VT_VALMASK | VT_LVAL)) == VT_CONST)
gv(RC_FLOAT);
/* must put at least one value in the floating point register */
if ((vtop[-1].r & VT_LVAL) &&
(vtop[0].r & VT_LVAL)) {
vswap();
gv(RC_FLOAT);
vswap();
}
swapped = 0;
/* swap the stack if needed so that t1 is the register and t2 is
the memory reference */
if (vtop[-1].r & VT_LVAL) {
vswap();
swapped = 1;
}
if (op >= TOK_ULT && op <= TOK_GT) {
/* load on stack second operand */
load(TREG_ST0, vtop);
save_reg(TREG_EAX); /* eax is used by FP comparison code */
if (op == TOK_GE || op == TOK_GT)
swapped = !swapped;
else if (op == TOK_EQ || op == TOK_NE)
swapped = 0;
if (swapped)
o(0xc9d9); /* fxch %st(1) */
o(0xe9da); /* fucompp */
o(0xe0df); /* fnstsw %ax */
if (op == TOK_EQ) {
o(0x45e480); /* and $0x45, %ah */
o(0x40fC80); /* cmp $0x40, %ah */
} else if (op == TOK_NE) {
o(0x45e480); /* and $0x45, %ah */
o(0x40f480); /* xor $0x40, %ah */
op = TOK_NE;
} else if (op == TOK_GE || op == TOK_LE) {
o(0x05c4f6); /* test $0x05, %ah */
op = TOK_EQ;
} else {
o(0x45c4f6); /* test $0x45, %ah */
op = TOK_EQ;
}
vtop--;
vtop->r = VT_CMP;
vtop->c.i = op;
} else {
/* no memory reference possible for long double operations */
if ((vtop->type.t & VT_BTYPE) == VT_LDOUBLE) {
load(TREG_ST0, vtop);
swapped = !swapped;
}
switch(op) {
default:
case '+':
a = 0;
break;
case '-':
a = 4;
if (swapped)
a++;
break;
case '*':
a = 1;
break;
case '/':
a = 6;
if (swapped)
a++;
break;
}
ft = vtop->type.t;
fc = vtop->c.ul;
if ((ft & VT_BTYPE) == VT_LDOUBLE) {
o(0xde); /* fxxxp %st, %st(1) */
o(0xc1 + (a << 3));
} else {
/* if saved lvalue, then we must reload it */
r = vtop->r;
if ((r & VT_VALMASK) == VT_LLOCAL) {
SValue v1;
r = get_reg(RC_INT);
v1.type.t = VT_INT;
v1.r = VT_LOCAL | VT_LVAL;
v1.c.ul = fc;
load(r, &v1);
fc = 0;
}
if ((ft & VT_BTYPE) == VT_DOUBLE)
o(0xdc);
else
o(0xd8);
gen_modrm(a, r, vtop->sym, fc);
}
vtop--;
}
}
// \delta v = (I - h J_v - h^2 J_q)^{-1} (h a + h^2 J_q v)
// \delta q = h (\delta v + v)
void vpSystem::IntegrateDynamicsBackwardEuler(scalar time_step)
{
if ( m_pRoot->m_bIsGround )
{
int i, j, n = m_sState.size();
if ( !n ) return;
ForwardDynamics();
RMatrix _a(n,1), _v(n,1);
RMatrix _Jq(n,n), _Jv(n,n);
for ( i = 0; i < n; i++ )
{
_a[i] = m_sState[i].GetAcceleration();
_v[i] = m_sState[i].GetVelocity();
}
for ( i = 0; i < n; i++ )
{
m_sState[i].SetDisplacement(m_sState[i].GetDisplacement() + LIE_EPS);
ForwardDynamics();
for ( j = 0; j < n; j++ ) _Jq(j,i) = (m_sState[j].GetAcceleration() - _a[j]) / LIE_EPS;
m_sState[i].SetDisplacement(m_sState[i].GetDisplacement() - LIE_EPS);
}
for ( i = 0; i < n; i++ )
{
m_sState[i].SetVelocity(m_sState[i].GetVelocity() + LIE_EPS);
ForwardDynamics();
for ( j = 0; j < n; j++ ) _Jv(j,i) = (m_sState[j].GetAcceleration() - _a[j]) / LIE_EPS;
m_sState[i].SetVelocity(m_sState[i].GetVelocity() - LIE_EPS);
}
_Jq *= (time_step * time_step);
_Jv *= -time_step;
_Jv -= _Jq;
for ( i = 0; i < n; i++ ) _Jv[i*(n+1)] += SCALAR_1;
_a *= time_step;
_a += _Jq * _v;
SolveAxEqualB_(_Jv, _a);
for ( i = 0; i < n; i++ ) m_sState[i].SetVelocity(_v[i] + _a[i]);
_a += _v;
_a *= time_step;
for ( i = 0; i < n; i++ ) m_sState[i].SetDisplacement(m_sState[i].GetDisplacement() + _a[i]);
} else
{
int i, j, n = m_sState.size(), m = n + 6;
ForwardDynamics();
RMatrix _a(m,1), _v(m,1), _dx(m,1);
RMatrix _Jq(m,m), _Jv(m,m), _M = Eye<scalar>(m,m);
SE3 _T;
se3 gv(SCALAR_0);
for ( i = 0; i < n; i++ )
{
_a[i] = m_sState[i].GetAcceleration();
_v[i] = m_sState[i].GetVelocity();
}
memcpy(&_a[n], &m_pRoot->m_sDV[0], sizeof(se3));
memcpy(&_v[n], &m_pRoot->m_sV[0], sizeof(se3));
for ( i = 0; i < n; i++ )
{
m_sState[i].SetDisplacement(m_sState[i].GetDisplacement() + LIE_EPS);
ForwardDynamics();
for ( j = 0; j < n; j++ ) _Jq(j,i) = (m_sState[j].GetAcceleration() - _a[j]) / LIE_EPS;
for ( j = 0; j < 6; j++ ) _Jq(n+j,i) = (m_pRoot->m_sDV[j] - _a[n+j]) / LIE_EPS;
m_sState[i].SetDisplacement(m_sState[i].GetDisplacement() - LIE_EPS);
}
for ( i = 0; i < 6; i++ )
{
gv[i] += LIE_EPS;
_T = m_pRoot->m_sFrame;
m_pRoot->m_sFrame *= Exp(gv);
ForwardDynamics();
for ( j = 0; j < n; j++ ) _Jq(j,n+i) = (m_sState[j].GetAcceleration() - _a[j]) / LIE_EPS;
for ( j = 0; j < 6; j++ ) _Jq(n+j,n+i) = (m_pRoot->m_sDV[j] - _a[n+j]) / LIE_EPS;
m_pRoot->m_sFrame = _T;
gv[i] = SCALAR_0;
}
for ( i = 0; i < n; i++ )
{
m_sState[i].SetVelocity(m_sState[i].GetVelocity() + LIE_EPS);
ForwardDynamics();
for ( j = 0; j < n; j++ ) _Jv(j,i) = (m_sState[j].GetAcceleration() - _a[j]) / LIE_EPS;
for ( j = 0; j < 6; j++ ) _Jv(n+j,i) = (m_pRoot->m_sDV[j] - _a[n+j]) / LIE_EPS;
m_sState[i].SetVelocity(m_sState[i].GetVelocity() - LIE_EPS);
}
for ( i = 0; i < 6; i++ )
{
m_pRoot->m_sV[i] += LIE_EPS;
ForwardDynamics();
for ( j = 0; j < n; j++ ) _Jv(j,n+i) = (m_sState[j].GetAcceleration() - _a[j]) / LIE_EPS;
for ( j = 0; j < 6; j++ ) _Jv(n+j,n+i) = (m_pRoot->m_sDV[j] - _a[n+j]) / LIE_EPS;
m_pRoot->m_sV[i] -= LIE_EPS;
}
_Jq *= (time_step * time_step);
_Jv *= time_step;
_M -= _Jq;
_M -= _Jv;
_a *= time_step;
_a += _Jq * _v;
SolveAxEqualB(_M, _dx, _a);
for ( i = 0; i < n; i++ ) m_sState[i].SetVelocity(m_sState[i].GetVelocity() + _dx[i]);
for ( i = 0; i < 6; i++ ) m_pRoot->m_sV[i] += _dx[n+i];
_dx += _v;
_dx *= time_step;
for ( i = 0; i < n; i++ ) m_sState[i].SetDisplacement(m_sState[i].GetDisplacement() + _dx[i]);
m_pRoot->m_sFrame *= Exp(se3(_dx[n], _dx[n+1], _dx[n+2], _dx[n+3], _dx[n+4], _dx[n+5]));
}
Reparameterize();
}
