short write_log(int *fileid_ptr, char *filename, char *message,
size_t message_len, off_t threshold, tbool wrap_flag,
tbool signal_on_thres_flag, off_t *ckpt_offset_ptr,
time_t *ckpt_mod_time_ptr, tbool msg_archive_flag)
/*
* fileid_ptr -- pointer to the file descriptor of log file
* filename -- name of the file
* message -- message buffer
* message_len -- length of buffer
* threshold -- max size/wrap threshold value
* wrap_flag -- flag which specifies whether to wrap or not if the
* threshold is met
* signal_on_thres_flag -- flag which specifies whether to send a signal
* to the parent process if the threshold is met
* ckpt_offset_ptr -- pointer to ckpt structure file offset save area
* ckpt_mod_time_ptr -- pointer to ckpt structure file modification time
* save area
*/
{
/*
*** Data and Functions Definitions/Declarations **************************
*/
char error_msg[128]; /* error message string */
extern char *program_name; /* this program's name (argv[0]) */
extern int ckpt_file; /* Checkpoint File File Descriptor */
int bytes_written; /* number of bytes transferred with write()*/
int errno_save; /* errno save area */
off_t file_offset; /* offset of the log file */
short exit_code; /* exit program return code */
/*
*** Beginning of Executable Code *****************************************
*/
exit_code = GOOD;
errno = 0;
if ((bytes_written = write(*fileid_ptr, message,
(unsigned int) message_len))
== -1)
{
errno_save = errno;
(void) sprintf(error_msg,
"\n%s -- error writing to %s.\nerrno: %d (%s).\n",
program_name,
filename,
errno_save,
strerror(errno_save));
(void) fprintf(stderr, "%s", error_msg);
(void) fflush(stderr);
exit_code = BAD_LOG_WRITE1;
}
else
{
if (bytes_written != message_len)
{
(void) sprintf(error_msg,
"\n%s -- error writing to %s.\n\
Only %d bytes out of a %zd byte buffer were written to disk.\n",
program_name,
filename,
bytes_written,
message_len);
(void) fprintf(stderr, "%s", error_msg);
(void) fflush(stderr);
exit_code |= BAD_LOG_WRITE2;
} /* endif */
errno = 0;
if (fsync(*fileid_ptr) != GOOD)
{
errno_save = errno;
(void) sprintf(error_msg,
"\n%s -- error on fsync() to %s.\nerrno: %d (%s).\n",
program_name,
filename,
errno_save,
strerror(errno_save));
(void) fprintf(stderr, "%s", error_msg);
(void) fflush(stderr);
exit_code |= BAD_LOG_FSYNC;
} /* endif -- fsync() */
/*
* check for log wrap condition
*/
if (check_wrap(fileid_ptr, filename, bytes_written, threshold, wrap_flag,signal_on_thres_flag, *ckpt_offset_ptr, &file_offset, msg_archive_flag) != GOOD)
{
exit_code |= BAD_WRAP_CHECK;
}
/*
* update ckpt structure and save to disk
*/
if (ckpt_file != -1)
{
if (update_ckpt(*fileid_ptr, filename, ckpt_offset_ptr, file_offset,ckpt_mod_time_ptr) != GOOD)
{
exit_code |= BAD_UPDATE_CKPT;
}
}
} /* endif -- write() */
return(exit_code);
} /* write_log() */
CMUK_ERROR_CODE cmuk::computeFootIK( LegIndex leg,
const vec3f& pos,
vec3f* q_bent_forward,
vec3f* q_bent_rearward ) const {
if ((int)leg < 0 || (int)leg >= NUM_LEGS) {
return CMUK_BAD_LEG_INDEX;
} else if (!q_bent_forward || !q_bent_rearward) {
return CMUK_INSUFFICIENT_ARGUMENTS;
}
debug << "*** computing IK...\n";
int hipflags = 0;
// subtract off hip position
vec3f p = pos - jo(_kc, leg, HIP_RX_OFFSET, _centeredFootIK);
vec3f orig = pos;
// get dist from hip rx joint to y rotation plane
const float& d = jo(_kc, leg, HIP_RY_OFFSET, _centeredFootIK)[1];
// get the squared length of the distance on the plane
float yz = p[1]*p[1] + p[2]*p[2];
// alpha is the angle of the foot in the YZ plane with respect to the Y axis
float alpha = atan2(p[2], p[1]);
// h is the distance of foot from hip in YZ plane
float h = sqrt(yz);
// beta is the angle between the foot-hip vector (projected in YZ
// plane) and the top hip link.
float cosbeta = d / h;
debug << "p = " << p << ", d = " << d << ", yz = " << yz << "\nalpha = " << alpha << ", h = " << h << ", cosbeta=" << cosbeta << "\n";
if (fabs(cosbeta) > 1) {
debug << "violated triangle inequality when calculating hip_rx_angle!\n" ;
if (fabs(cosbeta) - 1 > 1e-4) {
hipflags = hipflags | IK_UPPER_DISTANCE;
}
cosbeta = (cosbeta < 0) ? -1 : 1;
if (yz < 1e-4) {
p[1] = d;
p[2] = 0;
} else {
float scl = fabs(d) / h;
p[1] *= scl;
p[2] *= scl;
orig = p + jo(_kc, leg, HIP_RX_OFFSET, _centeredFootIK);
}
}
float beta = acos(cosbeta);
// Now compute the two possible hip angles
float hip_rx_angles[2], badness[2];
int flags[2];
flags[0] = hipflags;
flags[1] = hipflags;
hip_rx_angles[0] = fix_angle(alpha - beta, -M_PI, M_PI);
hip_rx_angles[1] = fix_angle(alpha + beta, -M_PI, M_PI);
const float& min = jl(_kc, leg, HIP_RX, 0);
const float& max = jl(_kc, leg, HIP_RX, 1);
// See how badly we violate the joint limits for this hip angles
for (int i=0; i<2; ++i) {
float& angle = hip_rx_angles[i];
badness[i] = fabs(compute_badness(angle, min, max));
if (badness[i]) { flags[i] = flags[i] | IK_UPPER_ANGLE_RANGE; }
}
// Put the least bad (and smallest) hip angle first
bool swap = false;
if ( badness[1] <= badness[0] ) {
// We want the less bad solution for hip angle
swap = true;
} else if (badness[0] == 0 && badness[1] == 0) {
// We want the solution for hip angle that leaves the hip up.
if ((leg == FL || leg == HL) && hip_rx_angles[0] > hip_rx_angles[1]) {
swap = true;
} else if ((leg == FR || leg == HR) && hip_rx_angles[0] < hip_rx_angles[1]) {
swap = true;
}
}
if (swap) {
std::swap(hip_rx_angles[0], hip_rx_angles[1]);
std::swap(badness[0], badness[1]);
std::swap(flags[0], flags[1]);
}
int hip_solution_cnt = 2;
if (badness[0] == 0 && badness[1] != 0) {
hip_solution_cnt = 1;
}
debug << "hip_rx_angles[0]=" << hip_rx_angles[0]
<< ", badness=" << badness[0]
<< ", flags=" << flags[0] << "\n";
debug << "hip_rx_angles[1]=" << hip_rx_angles[1]
<< ", badness=" << badness[1]
<< ", flags=" << flags[1] << "\n";
debug << "hip_solution_cnt = " << hip_solution_cnt << "\n";
vec3f qfwd[2], qrear[2];
for (int i=0; i<hip_solution_cnt; ++i) {
debug << "** computing ll solution " << (i+1) << " of " << (hip_solution_cnt) << "\n";
float hip_rx = hip_rx_angles[i];
// now make inv. transform to get rid of hip rotation
Transform3f tx = Transform3f::rx(hip_rx, jo(_kc, leg, HIP_RX_OFFSET, _centeredFootIK));
vec3f ptx = tx.transformInv(orig);
debug << "tx=[" << tx.translation() << ", " << tx.rotation() << "], ptx = " << ptx << "\n";
// calculate lengths for cosine law
float l1sqr = ol2(_kc, leg, KNEE_RY_OFFSET, _centeredFootIK);
float l2sqr = ol2(_kc, leg, FOOT_OFFSET, _centeredFootIK);
float l1 = ol(_kc, leg, KNEE_RY_OFFSET, _centeredFootIK);
float l2 = ol(_kc, leg, FOOT_OFFSET, _centeredFootIK);
float ksqr = ptx[0]*ptx[0] + ptx[2]*ptx[2];
float k = sqrt(ksqr);
debug << "l1=" << l1 << ", l2=" << l2 << ", k=" << k << "\n";
// check triangle inequality
if (k > l1 + l2) {
debug << "oops, violated the triangle inequality for lower segments: "
<< "k = " << k << ", "
<< "l1 + l2 = " << l1 + l2 << "\n";
if (k - (l1 + l2) > 1e-4) {
flags[i] = flags[i] | IK_LOWER_DISTANCE;
}
k = l1 + l2;
ksqr = k * k;
}
// 2*theta is the acute angle formed by the spread
// of the two hip rotations...
float costheta = (l1sqr + ksqr - l2sqr) / (2 * l1 * k);
if (fabs(costheta) > 1) {
debug << "costheta = " << costheta << " > 1\n";
if (fabs(costheta) - 1 > 1e-4) {
flags[i] = flags[i] | IK_LOWER_DISTANCE;
}
costheta = (costheta < 0) ? -1 : 1;
}
float theta = acos(costheta);
// gamma is the angle of the foot with respect to the z axis
float gamma = atan2(-ptx[0], -ptx[2]);
// hip angles are just offsets off of gamma now
float hip_ry_1 = gamma - theta;
float hip_ry_2 = gamma + theta;
// phi is the obtuse angle of the parallelogram
float cosphi = (l1sqr + l2sqr - ksqr) / (2 * l1 * l2);
if (fabs(cosphi) > 1) {
debug << "cosphi = " << cosphi << " > 1\n";
if (fabs(cosphi) - 1 > 1e-4) {
flags[i] = flags[i] | IK_LOWER_DISTANCE;
}
cosphi = (cosphi < 0) ? -1 : 1;
}
float phi = acos(cosphi);
// epsilon is the "error" caused by not having feet offset directly
// along the z-axis (if they were, epsilon would equal zero)
float epsilon = le(_kc, leg, _centeredFootIK);
// now we can directly solve for knee angles
float knee_ry_1 = M_PI - phi - epsilon;
float knee_ry_2 = -M_PI + phi - epsilon;
// now fill out angle structs and check limits
qfwd[i] = vec3f(hip_rx, hip_ry_1, knee_ry_1);
qrear[i] = vec3f(hip_rx, hip_ry_2, knee_ry_2);
debug << "before wrap, qfwd = " << qfwd[i] << "\n";
debug << "before wrap, qrear = " << qrear[i] << "\n";
check_wrap(_kc, qfwd[i], leg);
check_wrap(_kc, qrear[i], leg);
debug << "after wrap, qfwd = " << qfwd[i] << "\n";
debug << "after wrap, qrear = " << qrear[i] << "\n";
if (!check_limits(_kc, qfwd[i], leg)) {
debug << "violated limits forward!\n";
flags[i] = flags[i] | IK_LOWER_ANGLE_RANGE_FWD;
}
if (!check_limits(_kc, qrear[i], leg)) {
debug << "violated limits rearward!\n";
flags[i] = flags[i] | IK_LOWER_ANGLE_RANGE_REAR;
}
} // for each viable hip solution
int best = 0;
if (hip_solution_cnt == 2) {
if (howbad(flags[0]) > howbad(flags[1])) {
best = 1;
}
debug << "best overall solution is " << (best+1) << "\n";
}
*q_bent_forward = qfwd[best];
*q_bent_rearward = qrear[best];
return flags_to_errcode(flags[best]);
}
void display_indicator(long bytes)
{
int pct;
char ind[64];
char *p;
/* Indicator width */
static unsigned char ind_sizes[]={5, 0, 11, 5, 8, 11, 11};
#if SFX_LEVEL>=ARJ
static int prev_pct=0;
#endif
if(!file_packing)
return;
p=ind;
if(arjdisp_enabled)
arjdisp_scrn((unsigned long)bytes);
/* Different conditions for ARJ and ARJSFX! */
#if SFX_LEVEL>=ARJ
else if(indicator_style!=IND_NONE)
{
check_wrap(ind_sizes[indicator_style]);
if(uncompsize<0L)
{
if(bytes==0L)
p+=sprintf(p, sfmt_double, del_double);
p+=sprintf(p, sfmt_bytes, bytes, del_double);
}
else
{
if(indicator_style==IND_NORMAL||indicator_style==IND_TOTAL_PCT)
{
if(bytes==0L)
{
p+=sprintf(p, sfmt_single, del_single);
p+=sprintf(p, sfmt_start_graph, del_single);
}
else
{
if(total_size!=0&&display_totals&&indicator_style==IND_TOTAL_PCT)
pct=calc_percentage(total_written+bytes, total_size);
else
pct=calc_percentage(bytes, uncompsize);
if(pct==prev_pct&&CHECK_SENTRY())
return;
p+=sprintf(p, sfmt_numeric, pct/10, del_single);
}
}
else if(indicator_style==IND_GRAPH||indicator_style==IND_TOTAL_GRAPH)
{
if(bytes==0L)
{
p+=sprintf(p, sfmt_double, del_double);
p+=sprintf(p, sfmt_graph, del_double);
*p='\0'; p=ind;
msg_cprintf(H_HL, (FMSG *)strform, ind);
}
else
{
if(total_size!=0&&display_totals&&indicator_style==IND_TOTAL_GRAPH)
pct=calc_percentage(total_written+bytes, total_size);
else
pct=calc_percentage(bytes, uncompsize);
if(pct==prev_pct&&CHECK_SENTRY())
return;
p=nputnc(p, COUNTER_CHAR, pct/100);
p=nputnc(p, '\b', pct/100);
*p='\0'; p=ind;
msg_cprintf(H_OPER, (FMSG *)strform, ind);
}
}
else if(indicator_style==IND_PCT_GRAPH||indicator_style==IND_TOTAL_PCT_GRAPH||indicator_style==IND_TOTAL_PCT_LGRAPH)
{
if(total_size!=0&&display_totals&&(indicator_style==IND_TOTAL_PCT_GRAPH||indicator_style==IND_TOTAL_PCT_LGRAPH))
pct=calc_percentage(total_written+bytes, total_size);
else
pct=calc_percentage(bytes, uncompsize);
if(bytes==0L)
{
p+=sprintf(p, sfmt_double, del_double);
p+=sprintf(p, sfmt_start_num, pct/10);
*p='\0'; p=ind;
if(pct==prev_pct&&CHECK_SENTRY())
return;
msg_cprintf(H_OPER, (FMSG *)strform, ind);
msg_cprintf(H_HL, sfmt_mid_graph, del_double);
}
else
{
p+=sprintf(p, sfmt_short_numeric, pct/10);
if(total_size!=0&&indicator_style==IND_TOTAL_PCT_GRAPH)
pct=calc_percentage(total_written+bytes, total_size);
else
pct=calc_percentage(bytes, uncompsize);
if(pct==prev_pct&&CHECK_SENTRY())
return;
p=nputnc(p, COUNTER_CHAR, pct/200);
p=nputnc(p, '\b', pct/200+5);
}
}
}
}
*p='\0';
msg_cprintf(H_OPER, (FMSG *)strform, ind);
prev_pct=pct;
SET_SENTRY();
#elif SFX_LEVEL==ARJSFXV
else if(indicator_style==IND_NORMAL||indicator_style==IND_GRAPH)
