void rk4::fParse(FILE *in){
int i;
string hold;
n = getint(in);
variables = new string[n + 1];
equations = ((fxn **) new int[n]);
iv = new double[n + 1];
variables[0] = getstring(in);
for(i = 1; i < n + 1; i++){
variables[i] = getstring(in);
// cout << variables[i] << endl;
}
for(i = 1; i < n + 1; i++){
iv[i] = getdouble(in);
}
a = getdouble(in);
b = getdouble(in);
iv[0] = a;
p = getint(in);
h = (b-a)/p;
for(i = 0; i < n; i++){
hold = getstring(in);
equations[i] = new fxn(hold);
//cout << "Noob" << endl;
equations[i]->fxnVars(variables, n+1);
cout << equations[i]->fxnPrint() << endl;
}
}
obj_t *tplane_init(FILE *fp, char *objclass) {
/****variables****/
obj_t *obj;
triple_t projected;
tplane_t *tplane;
plane_t *plane;
/*****************/
obj = plane_init(fp, objclass);
plane = (plane_t *)(obj->typeData);
tplane = (tplane_t *) malloc(sizeof(tplane_t));
plane->priv = tplane;
gettriple(fp, &tplane->orientation);
getdouble(fp, &tplane->tileWidth);
getdouble(fp, &tplane->tileHeight);
tplane->orientation = tl_unitvec3(&tplane->orientation);
projected = tl_project3(&plane->normal, &tplane->orientation);
projected = tl_unitvec3(&projected);
tplane->rotate[0] = projected;
tplane->rotate[2] = tl_unitvec3(&plane->normal);
tplane->rotate[1] = tl_cross3(&tplane->rotate[2], &tplane->rotate[0]);
gettriple(fp, &tplane->bkgrndcolor);
getdouble(fp, &tplane->bkgrnddiff);
getdouble(fp, &tplane->bkgrndspec);
/* Override default pointers to surface color routines */
obj->sceneData->getcolor = tp_getcolor;
obj->sceneData->getdiffuse = tp_getdiffuse;
obj->sceneData->getspecular = tp_getspecular;
return(obj);
}
void xml_proc(struct xml_in *x)
{
double a;
while (1) {
switch (xml_in_nibble(x)) {
case XML_IN_NONE:
return;
case XML_IN_TAG_START:
#if (0)
{
int i;
printf("start tag: %s, %d attributes\n", x->a, x->n);
for (i = 0; i < x->n; i++) {
printf(" %s %s \n", x->attr[i].name, x->attr[i].value);
}
}
#endif
if (strcmp("gmk", x->a) == 0) {
printf(" %s %s \n", x->attr[0].name, x->attr[0].value);
if (getdouble(&a, "id", x)) {
gmk.id = a;
printf("id= %f\n", gmk.id);
}
if (getdouble(&a, "crcOK", x)) {
gmk.crc = a;
printf("crc= %f\n", gmk.crc);
}
}
if (strcmp("pos3d", x->a) == 0) {
(getdouble(&gmk.x, "x", x));
(getdouble(&gmk.y, "y", x));
(getdouble(&gmk.z, "z", x));
}
if (strcmp("rot3d", x->a) == 0) {
(getdouble(&gmk.omega, "Omega", x));
(getdouble(&gmk.phi, "Phi", x));
(getdouble(&gmk.kappa, "Kappa", x));
}
if (strcmp("vision", x->a) == 0) {
int i, ix;
for (i = 0; i < x->n; i++) {
ix = atoi(x->attr[i].name + 3);
if (ix > -1 && ix < 10)
visionpar[ix] = atof(x->attr[i].value);
}
}
break;
case XML_IN_TAG_END:
//printf("end tag: %s\n", x->a);
break;
case XML_IN_TEXT:
//printf("text: %d bytes\n \"", x->n);
//fwrite(x->a, 1, x->n, stdout);
//printf("\"\n");
break;
}
}
}
double getdouble(double *pn) {
int i = 0, e = 0;
double b, c, sign;
while ((c = getch()) == ' ' || c == '\t')
;
if (!isdigit(c) && c != EOF && c != '+' && c != '-' && c != '\n' && c != '.')
c = getdouble(&array[++n]);
sign = (c == '-') ? -1 : 1;
if (c == '+' || c == '-') {
if (isdigit(b = getch()))
c = b;
else
c = getdouble(&array[++n]);
}
if (c == '\n') {
printf("\n");
c = getdouble(&array[++n]);
}
if (c != EOF) {
*pn = 0;
i = 10 * i + (c - '0');
while (isdigit(c = getch()))
i = 10 * i + (c - '0');
if (c == '.') {
while(isdigit(c = getch())) {
*pn = 10.0 * *pn + (c - '0');
e--;
}
}
while (e < 0) {
*pn *= 0.1;
e++;
}
*pn += i;
*pn *= sign;
ungetch(c);
}
return c;
}
/* Get capacitance in F */
double getcapacitance (const char *display)
{
double v;
int i;
while (1)
{
v = getdouble (display);
i = getint ("\n1)F 2)mF 3)μF 4)nF 5)pF : ");
switch (i)
{
case 1:
return v;
case 2:
return v * 1e-3;
case 3:
return v * 1e-6;
case 4:
return v * 1e-9;
case 5:
return v * 1e-12;
}
}
}
/* Get frequency in Hz*/
double getfrequency (const char *display)
{
double v;
int i;
while (1)
{
v = getdouble (display);
i = getint ("\n1)Hz 2)KHz 3)MHz 4)GHz : ");
switch (i)
{
case 1:
return v;
case 2:
return v * 1e3;
case 3:
return v * 1e6;
case 4:
return v * 1e9;
}
}
}
/* Get power in W */
double getpower (const char *display)
{
double v;
int i;
while (1)
{
v = getdouble (display);
i = getint ("\n1)MW 2)KW 3)W 4)mW 5)μW : ");
switch (i)
{
case 1:
return v * 1e6;
case 2:
return v * 1e3;
case 3:
return v;
case 4:
return v * 1e-3;
case 5:
return v * 1e-6;
}
}
}
/* Get voltage in V */
double getvoltage (const char *display)
{
double v;
int i;
while (1)
{
v = getdouble (display);
i = getint ("\n1)MV 2)KV 3)V 4)mV 5)μV 6)nV: ");
switch (i)
{
case 1:
return v * 1e6;
case 2:
return v * 1e3;
case 3:
return v;
case 4:
return v * 1e-3;
case 5:
return v * 1e-6;
case 6:
return v * 1e-9;
}
}
}
int main(void) {
double base;
int exponente;
base = getdouble("Introducir la base: ");
exponente = getint("Introducir el exponente: ");
printf("(%f)^(%d) = %g\n", base, exponente, calcPotencia(base,exponente));
}
/* Get amperage in A */
double getamperage (const char *display)
{
double v;
int i;
while (1)
{
v = getdouble (display);
i = getint ("\n1)KA 2)A 3)mA 4)μA 5)nA: ");
switch (i)
{
case 1:
return v * 1e3;
case 2:
return v;
case 3:
return v * 1e-3;
case 4:
return v * 1e-6;
case 5:
return v * 1e-9;
}
}
}
/* Get resistance in Ω */
double getresistance (const char *display)
{
double v;
int i;
while (1)
{
v = getdouble (display);
i = getint ("\n1)MΩ 2)KΩ 3)Ω 4)mΩ : ");
switch (i)
{
case 1:
return v * 1e6;
case 2:
return v * 1e3;
case 3:
return v;
case 4:
return v * 1e-3;
}
}
}
/* Get inductance in H */
double getinductance (const char *display)
{
double v;
int i;
while (1)
{
v = getdouble (display);
i = getint ("\n1)H 2)mH 3)μH 4)nH 5)pH : ");
switch (i)
{
case 1:
return v;
case 2:
return v * 1e-3;
case 3:
return v * 1e-6;
case 4:
return v * 1e-9;
case 5:
return v * 1e-12;
}
}
}
int
main() {
float f;
double d;
long double ld;
while ((f = getfloat()) != 0.0f) {
printf("%f %f %Lf\n", f, getdouble(), getlongdouble());
}
return 0;
}
int main(){
double v,x;
double t=0;
double h=DT;
fprintf(stderr, "初速度を入力");
if(getdouble(&v)==EOF)
return 1;
fprintf(stderr, "初期高度を入力してください");
if(getdouble(&x)==EOF)
return 1;
printf("%f\t%f\t%f\n",t,x,v);
//自由落下の計算
while(x>0){
t+=h;
v+=G*h;
x-=v*h;
printf("%f\t%f\t%f\n",t,x,v);
}
return 0;
}
obj_t *sphere_init(FILE *fp, char *objclass){
/****variables****/
obj_t *obj;
sphere_t *sphere;
/*****************/
obj = sceneobj_init(fp, objclass);
obj->sceneData->hits = sphere_hits;
sphere = (sphere_t *) malloc(sizeof(sphere_t));
gettriple(fp, &sphere->center);
getdouble(fp, &sphere->radius);
obj->typeData = sphere;
return obj;
}
/* Get impedance r + jx */
double complex getimpedance (const char *display)
{
double complex v;
double r;
double x;
char *rs, *rx;
rs = malloc (strlen (display) + 10);
rx = malloc (strlen (display) + 10);
strcpy (rs, display);
strcpy (rx, display);
strcat (rs, " - r : ");
strcat (rx, " - jx : ");
r = getdouble (rs);
x = getdouble (rx);
v = r + x * I;
if (rs)
free (rs);
if (rx)
free (rx);
return v;
}
int main(){
int c;
double p;
double* pp;
printf("Version prints the float read, or \"no float\" if no float\n");
printf("Float style \".4\" not allowed\n");
printf("no overflow check for largest allowed float\n\n");
pp = &p;
while ( ( c = getdouble(pp)) != EOF)
(c == 0)?printf("no float\n"):printf("type:%d, value:%g\n", c, p);
return 0;
}
/* scan_graph_mode:
* Prepare the graph and data structures depending on the layout mode.
* If Reduce is true, eliminate singletons and trees. Since G may be a
* subgraph, we remove the nodes from the root graph.
* Return the number of nodes in the reduced graph.
*/
int scan_graph_mode(graph_t * G, int mode)
{
int i, lenx, nV, nE, deg;
char *str;
node_t *np, *xp, *other;
double total_len = 0.0;
if (Verbose)
fprintf(stderr, "Scanning graph %s, %d nodes\n", G->name,
agnnodes(G));
/* Eliminate singletons and trees */
if (Reduce) {
for (np = agfstnode(G); np; np = xp) {
xp = agnxtnode(G, np);
deg = degreeKind(G, np, &other);
if (deg == 0) { /* singleton node */
agdelete(G->root, np);
} else if (deg == 1) {
agdelete(G->root, np);
xp = prune(G, other, xp);
}
}
}
nV = agnnodes(G);
nE = agnedges(G);
lenx = agindex(G->root->proto->e, "len");
if (mode == MODE_KK) {
Epsilon = .0001 * nV;
getdouble(G, "epsilon", &Epsilon);
if ((str = agget(G->root, "Damping")))
Damping = atof(str);
else
Damping = .99;
GD_neato_nlist(G) = N_NEW(nV + 1, node_t *);
for (i = 0, np = agfstnode(G); np; np = agnxtnode(G, np)) {
GD_neato_nlist(G)[i] = np;
ND_id(np) = i++;
ND_heapindex(np) = -1;
total_len += setEdgeLen(G, np, lenx);
}
} else {
void
Satellite::tle(const char *nm, const char *l1, const char *l2)
{
name = nm ;
// direct quantities from the orbital elements
N = getlong(l2, 2, 7) ;
YE = getlong(l1, 18, 20) ;
if (YE < 58)
YE += 2000 ;
else
YE += 1900 ;
TE = getdouble(l1, 20, 32) ;
M2 = RADIANS(getdouble(l1, 33, 43)) ;
IN = RADIANS(getdouble(l2, 8, 16)) ;
RA = RADIANS(getdouble(l2, 17, 25)) ;
EC = getdouble(l2, 26, 33)/1e7f ;
WP = RADIANS(getdouble(l2, 34, 42)) ;
MA = RADIANS(getdouble(l2, 43, 51)) ;
MM = 2.0f * M_PI * getdouble(l2, 52, 63) ;
RV = getlong(l2, 63, 68) ;
// derived quantities from the orbital elements
// convert TE to DE and TE
DE = fnday(YE, 1, 0) + (long) TE ;
TE -= (long) TE ;
N0 = MM/86400 ;
A_0 = pow(GM/(N0*N0), 1./3.) ;
B_0 = A_0*sqrt(1.-EC*EC) ;
PC = RE*A_0/(B_0*B_0) ;
PC = 1.5f*J2*PC*PC*MM ;
double CI = cos(IN) ;
QD = -PC*CI ;
WD = PC*(5*CI*CI-1)/2 ;
DC = -2*M2/(3*MM) ;
}
/*
* G E T S O L D A T A
*
* Obtain 'num' data items from input card(s).
* The first input card is already in global 'scard'.
*
* Returns -
* 0 OK
* -1 failure
*/
int
getsoldata(double *dp, int num, int solid_num)
{
int cd;
double *fp;
int i;
int j;
fp = dp;
for ( cd=1; num > 0; cd++ ) {
if ( cd != 1 ) {
if ( get_line( scard, sizeof(scard), "solid continuation card" ) == EOF ) {
printf("too few cards for solid %d\n",
solid_num);
return(-1);
}
/* continuation card
* solid type should be blank
*/
if ( (version==5 && scard[5] != ' ' ) ||
(version==4 && scard[3] != ' ' ) ) {
printf("solid %d (continuation) card %d non-blank\n",
solid_num, cd);
return(-1);
}
}
if ( num < 6 )
j = num;
else
j = 6;
for ( i=0; i<j; i++ ) {
*fp++ = getdouble( scard, 10+i*10, 10 );
}
num -= j;
}
return(0);
}
command()
{
double tval;
int i,echeck;
int pmin,pmax,pstep;
while (echeck = getstring(": ")) {
if (echeck == EOF) {
fileinput(EOF);
}
else if (in[0] == '\0') {
errprint("");
}
else if (startsame(in,"cycle")) {
cycle();
}
else if (startsame(in,"clear")) {
clear();
}
else if (startsame(in,"coarticulation")) {
getint(&coartflag);
}
else if (startsame(in,"rc")) {
zarrays();
cycle();
}
else if (startsame(in,"wordacts")) {
scr_words(printmin,printmax,3,0,"MAX");
}
else if (startsame(in,"wtacts")) {
scr_words(printmin,printmax,3,0,"ALL");
}
else if (startsame(in,"owtacts")) {
getstring("word: ");
scr_words(printmin,printmax,3,0,in);
}
else if (startsame(in,"phonacts")) {
scr_phonemes(printmin,printmax,3,0);
}
else if (startsame(in,"featacts")) {
scr_features();
}
else if (startsame(in,"sfeatacts")) {
getstring("fname: ");
sfeatacts(in);
}
else if (startsame(in,"memo")) {
getstring("string: ");
strcpy(memo,in);
}
else if (startsame(in,"expr")) {
setex();
}
else if (startsame(in,"fcopt")) {
getint(&fcflag);
}
else if (startsame(in,"fpcyc")) {
getint(&fpcyc);
}
else if (startsame(in,"finput")) {
fileinput(NONSTD);
}
else if (startsame(in,"inoise")) {
getval(&inoise);
}
else if (startsame(in,"inspecs")) {
getstring("File name (- = stdin): ");
inspecs(in);
}
else if (startsame(in,"infeatures")) {
getstring("File name: ");
infeats(in);
}
/* NOT PRESENTLY OPERATIVE -- JLM 10-5-82
else if (startsame(in,"wsubset")) {
wordsubset();
}
*/
else if (startsame(in,"test")) {
getstring("test string: ");
strcpy(memo,in);
test(in);
}
else if (startsame(in,"topdown")) {
topdown();
}
else if (startsame(in,"output")) {
setout();
}
else if (startsame(in,"ofile")) {
getstring("give filename (or - for none): ");
setoutfile(in);
}
else if (in[0] == '?') {
help();
}
else if (startsame(in,"help")) {
help();
}
else if (startsame(in,"lexicon")) {
getlex();
}
else if (startsame(in,"params")) {
getpars();
}
else if (startsame(in,"quit")) {
quit();
}
else if (startsame(in,"decay")) {
getdouble(decay,NLEVS,levlabs);
}
else if (startsame(in,"alpha")) {
getdouble(alpha,NPARAMS,conlabs);
}
else if (startsame(in,"gamma")) {
getdouble(ga,NLEVS,levlabs);
}
else if (startsame(in,"grace")) {
getint(&grace);
}
else if (startsame(in,"rest")) {
tval = rest[W];
getdouble(rest,NLEVS,levlabs);
if (tval != rest[W]) {
initialize();
}
}
else if (startsame(in,"fweight")) {
getdouble(fweight,NCONTINS,contname);
}
else if (startsame(in,"pthresh")) {
getdouble(pthresh,NLEVS,levlabs);
}
else if (startsame(in,"ngraph")) {
newgraph(pmin,ng_max,pstep);
}
else if (startsame(in,"ngmax")) {
getint(&ng_max);
}
else if (startsame(in,"ngwscale")) {
getval(&ng_wscale);
}
else if (startsame(in,"ngsscale")) {
getval(&ng_sscale);
}
else if (startsame(in,"ngpscale")) {
getval(&ng_pscale);
}
else if (startsame(in,"nreps")) {
getint(&nreps);
}
else if (startsame(in,"pfreq")) {
getint(&printfreq);
}
else if (startsame(in,"rarate")) {
getval(&rarate);
}
else if (startsame(in,"sumpr")) {
scr_sum(pmin,pmax,pstep);
}
else if (startsame(in,"sinspec")) {
sinspec();
}
else if (startsame(in,"sfeatures")) {
getstring("Filename: ");
sfeatures(in);
}
else if (startsame(in,"dinspec")) {
dinspec();
}
else if (startsame(in,"sumopt")) {
getint(&sumflag);
}
else if (startsame(in,"pmin")) {
getint(&pmin);
}
else if (startsame(in,"pmax")) {
getint(&pmax);
}
else if (startsame(in,"pstep")) {
getint(&pstep);
}
else if (startsame(in,"min")) {
getval(&min);
}
else if (startsame(in,"max")) {
getval(&max);
}
else if (startsame(in,"windowcent")) {
getval(&windowcent);
}
else if (startsame(in,"wbase")) {
getval(&wbase);
}
else if (startsame(in,"wgraph")) {
wgraph(pmin,ng_max,pstep);
}
else if (startsame(in,"wchange")) {
getval(&wchange);
}
else if (startsame(in,"wgain")) {
getval(&wgain);
}
else if (startsame(in,"wramp")) {
getval(&wramp);
}
else if (startsame(in,"imax")) {
getval(&imax);
}
else if (startsame(in,"sscale")) {
getval(&sscale);
}
else if (startsame(in,"nsscale")) {
getval(&nsscale);
}
else if (startsame(in,"freqscale")) {
tval = fscale;
getval(&fscale);
if (tval != fscale) {
initialize();
}
}
else if (startsame(in,"abort")) {
abort(); /* to get a core dump for sdb */
}
else {
errprint("Unrecognized request: For help type ?.");
if (infp != stdin) fileinput(STD);
}
wait(0);
}
}
///////////////////////////////////////////////////////////////////////////////
// MAIN
///////////////////////////////////////////////////////////////////////////////
int main( int argc, char** argv ) {
// Command line inputs
bool show_gui = false;
bool use_ros = false;
walktype walk_type = walk_canned;
double walk_circle_radius = 5.0;
double walk_dist = 20;
double footsep_y = 0.085; // half of horizontal separation distance between feet
double foot_liftoff_z = 0.05; // foot liftoff height
double step_length = 0.15;
bool walk_sideways = false;
double com_height = 0.48; // height of COM above ANKLE
double com_ik_ascl = 0;
double zmpoff_y = 0; // lateral displacement between zmp and ankle
double zmpoff_x = 0;
double lookahead_time = 2.5;
double startup_time = 1.0;
double shutdown_time = 1.0;
double double_support_time = 0.05;
double single_support_time = 0.70;
size_t max_step_count = 4;
double zmp_jerk_penalty = 1e-8; // jerk penalty on ZMP controller
ik_error_sensitivity ik_sense = ik_strict;
// Parse command line inputs
const struct option long_options[] = {
{ "show-gui", no_argument, 0, 'g' },
{ "use-ros", no_argument, 0, 'R' },
{ "ik-errors", required_argument, 0, 'I' },
{ "walk-type", required_argument, 0, 'w' },
{ "walk-distance", required_argument, 0, 'D' },
{ "walk-circle-radius", required_argument, 0, 'r' },
{ "max-step-count", required_argument, 0, 'c' },
{ "foot-separation-y", required_argument, 0, 'y' },
{ "foot-liftoff-z", required_argument, 0, 'z' },
{ "step-length", required_argument, 0, 'l' },
{ "walk-sideways", no_argument, 0, 'S' },
{ "com-height", required_argument, 0, 'h' },
{ "comik-angle-weight", required_argument, 0, 'a' },
{ "zmp-offset-y", required_argument, 0, 'Y' },
{ "zmp-offset-x", required_argument, 0, 'X' },
{ "lookahead-time", required_argument, 0, 'T' },
{ "startup-time", required_argument, 0, 'p' },
{ "shutdown-time", required_argument, 0, 'n' },
{ "double-support-time", required_argument, 0, 'd' },
{ "single-support-time", required_argument, 0, 's' },
{ "zmp-jerk-penalty", required_argument, 0, 'P' },
{ "help", no_argument, 0, 'H' },
{ 0, 0, 0, 0 },
};
const char* short_options = "gRI:w:D:r:c:y:z:l:Sh:a:Y:X:T:p:n:d:s:P:H";
int opt, option_index;
while ( (opt = getopt_long(argc, argv, short_options, long_options, &option_index)) != -1 ) {
switch (opt) {
case 'g': show_gui = true; break;
case 'R': use_ros = true; break;
case 'I': ik_sense = getiksense(optarg); break;
case 'w': walk_type = getwalktype(optarg); break;
case 'D': walk_dist = getdouble(optarg); break;
case 'r': walk_circle_radius = getdouble(optarg); break;
case 'c': max_step_count = getlong(optarg); break;
case 'y': footsep_y = getdouble(optarg); break;
case 'z': foot_liftoff_z = getdouble(optarg); break;
case 'l': step_length = getdouble(optarg); break;
case 'S': walk_sideways = true; break;
case 'h': com_height = getdouble(optarg); break;
case 'a': com_ik_ascl = getdouble(optarg); break;
case 'Y': zmpoff_y = getdouble(optarg); break;
case 'X': zmpoff_x = getdouble(optarg); break;
case 'T': lookahead_time = getdouble(optarg); break;
case 'p': startup_time = getdouble(optarg); break;
case 'n': shutdown_time = getdouble(optarg); break;
case 'd': double_support_time = getdouble(optarg); break;
case 's': single_support_time = getdouble(optarg); break;
case 'P': zmp_jerk_penalty = getdouble(optarg); break;
case 'H': usage(std::cout); exit(0); break;
default: usage(std::cerr); exit(1); break;
}
}
/* Initialize ROS */
double frequency = 200;
//FIXME: ROS dependent
if(use_ros) {
ros::init( argc, argv, "publish_and_readonce" );
rosnode = new ros::NodeHandle();
loop_rate = new ros::Rate(frequency);
ros::Time last_ros_time_;
// Wait until sim is active (play)
bool wait = true;
while( wait ) {
last_ros_time_ = ros::Time::now();
if( last_ros_time_.toSec() > 0 ) {
wait = false;
}
}
// init ros joints
RosJointInit();
// ros topic subscribtions
ros::SubscribeOptions jointStatesSo =
ros::SubscribeOptions::create<sensor_msgs::JointState>(
"/atlas/joint_states", 1, GetJointStates,
ros::VoidPtr(), rosnode->getCallbackQueue());
jointStatesSo.transport_hints = ros::TransportHints().unreliable();
ros::Subscriber subJointStates = rosnode->subscribe(jointStatesSo);
pub_joint_commands_ =
rosnode->advertise<osrf_msgs::JointCommands>(
"/atlas/joint_commands", 1, true);
}
/* Initialize AK */
if(!_ak) {
DartLoader dart_loader;
World *mWorld = dart_loader.parseWorld(ATLAS_DATA_PATH "atlas/atlas_world.urdf");
_atlas = mWorld->getSkeleton("atlas");
_ak = new AtlasKinematics();
_ak->init(_atlas);
}
_atlas->setPose(_atlas->getPose().setZero(), true);
AtlasKinematics *AK = _ak;
/* Begin generating trajectories */
/* Trajectory that stores dof ticks */
vector<VectorXd> joint_traj;
/* Setup dofs initial conditions */
/* Relax */
VectorXd dofs = _atlas->getPose().setZero();
_atlas->setPose(dofs, true);
const int relax_ticks = 1000;
Relax(AK, _atlas, dofs, joint_traj, relax_ticks);
/* Walking variables */
IK_Mode mode[NUM_MANIPULATORS];
mode[MANIP_L_FOOT] = IK_MODE_SUPPORT;
mode[MANIP_R_FOOT] = IK_MODE_WORLD;
mode[MANIP_L_HAND] = IK_MODE_FIXED;
mode[MANIP_R_HAND] = IK_MODE_FIXED;
Vector3d comDelta = Vector3d::Zero();
Vector3d leftDelta = Vector3d::Zero();
Vector3d rightDelta = Vector3d::Zero();
int N = 0;
Matrix4d Twm[NUM_MANIPULATORS];
Twm[MANIP_L_FOOT] = AK->getLimbTransB(_atlas, MANIP_L_FOOT);
Twm[MANIP_R_FOOT] = AK->getLimbTransB(_atlas, MANIP_R_FOOT);
Matrix4d Twb;
Twb.setIdentity();
VectorXd dofs_save;
/* Move COM down */
comDelta << 0, 0, -0.04;
leftDelta.setZero();
rightDelta.setZero();
const int com_ticks = 1000;
genCOMIKTraj(AK, _atlas, Twb, Twm, dofs, comDelta, leftDelta, rightDelta, joint_traj, com_ticks);
/* ZMP Walking */
/* ZMP parameters */
// number of steps to walk
int numSteps = 12;
// lenght of a half step
double stepLength = 0.15;
// half foot seperation
dofs_save = _atlas->getPose();
cout << "********************************************" << endl;
cout << "Start ZMP walking" << endl;
cout << "*************************************" << endl;
cout << "POS ERROR: " << (dofs_save - dofs).norm() << endl;
cout << "*************************************" << endl;
_atlas->setPose(dofs);
double footSeparation = (AK->getLimbTransW(_atlas, Twb, MANIP_L_FOOT)(1,3) -
AK->getLimbTransW(_atlas, Twb, MANIP_R_FOOT)(1,3) ) / 2;
cout << "Half foot seperation: " << footSeparation << endl;
// one step time
double stepDuration = 1.0;
// move ZMP time
double slopeTime = 0.15;
// keep ZMP time
double levelTime = 0.85;
// command sending period
double dt = 1/frequency;
// height of COM
double zg = AK->getCOMW(_atlas, Twb)(2) - AK->getLimbTransW(_atlas, Twb, MANIP_L_FOOT)(2,3);
cout << "zg " << zg << endl;
// preview how many steps
int numPreviewSteps = 2;
// double Qe = 1;
// double R = 1e-6;
double Qe = 1e7;
double R = 10;
/****************************************
* Generate joint trajectory
***************************************/
gZU.setParameters( dt, 9.81, dofs );
gZU.generateZmpPositions( numSteps, true,
stepLength, footSeparation,
stepDuration,
slopeTime,
levelTime );
gZU.getControllerGains( Qe, R, zg, numPreviewSteps );
gZU.generateCOMPositions();
gZU.getJointTrajectories();
gZU.print( "jointsWholeBody.txt", gZU.mWholeBody );
gZU.mDartDofs;
joint_traj.insert(joint_traj.end(), gZU.mDartDofs.begin(), gZU.mDartDofs.end());
// Bake me some GUI viz
FileInfoSkel<SkeletonDynamics> model;
model.loadFile(ATLAS_DATA_PATH"/skel/ground1.skel", SKEL);
SkeletonDynamics *ground = dynamic_cast<SkeletonDynamics *>(model.getSkel());
ground->setName("ground");
vector<SkeletonDynamics *> skels;
skels.push_back(ground);
skels.push_back(_atlas);
ZmpGUI gui(skels);
gui.bake(joint_traj);
glutInit(&argc, argv);
gui.initWindow(640, 480, "Atlas ZMP Walking");
glutMainLoop();
/**************************************
* Publish joint trajectory
****************************************/
//FIXME: ROS Dependent
//MoveJointTractory(AK, _atlas, dofs, gZU.mWholeBody, 20, 20, 20, 1.2, 1.2, 1.2);
return 0;
}
int printfcmd(int argc, char *argv[])
{
char *fmt;
char *format;
int ch;
rval = 0;
nextopt(nullstr);
argv = argptr;
format = *argv;
if (!format) {
warnx("usage: printf format [arg ...]");
goto err;
}
gargv = ++argv;
#define SKIP1 "#-+ 0"
#define SKIP2 "*0123456789"
do {
/*
* Basic algorithm is to scan the format string for conversion
* specifications -- once one is found, find out if the field
* width or precision is a '*'; if it is, gather up value.
* Note, format strings are reused as necessary to use up the
* provided arguments, arguments of zero/null string are
* provided to use up the format string.
*/
/* find next format specification */
for (fmt = format; (ch = *fmt++) ;) {
char *start;
char nextch;
int array[2];
int *param;
if (ch == '\\') {
int c_ch;
fmt = conv_escape(fmt, &c_ch);
ch = c_ch;
goto pc;
}
if (ch != '%' || (*fmt == '%' && (++fmt || 1))) {
pc:
blt_putchar(ch);
continue;
}
/* Ok - we've found a format specification,
Save its address for a later blt_printf(). */
start = fmt - 1;
param = array;
/* skip to field width */
fmt += strspn(fmt, SKIP1);
if (*fmt == '*')
*param++ = getintmax();
/* skip to possible '.', get following precision */
fmt += strspn(fmt, SKIP2);
if (*fmt == '.')
++fmt;
if (*fmt == '*')
*param++ = getintmax();
fmt += strspn(fmt, SKIP2);
ch = *fmt;
if (!ch) {
warnx("missing format character");
goto err;
}
/* null terminate format string to we can use it
as an argument to printf. */
nextch = fmt[1];
fmt[1] = 0;
switch (ch) {
case 'b': {
int done = conv_escape_str(getstr());
char *p = stackblock();
*fmt = 's';
PF(start, p);
/* escape if a \c was encountered */
if (done)
goto out;
*fmt = 'b';
break;
}
case 'c': {
int p = getchr();
PF(start, p);
break;
}
case 's': {
char *p = getstr();
PF(start, p);
break;
}
case 'd':
case 'i': {
intmax_t p = getintmax();
char *f = mklong(start, fmt);
PF(f, p);
break;
}
case 'o':
case 'u':
case 'x':
case 'X': {
uintmax_t p = getuintmax();
char *f = mklong(start, fmt);
PF(f, p);
break;
}
case 'e':
case 'E':
case 'f':
case 'g':
case 'G': {
double p = getdouble();
PF(start, p);
break;
}
default:
warnx("%s: invalid directive", start);
goto err;
}
*++fmt = nextch;
}
} while (gargv != argv && *gargv);
out:
return rval;
err:
return 1;
}
int
read_arbn(char *name)
{
int npt; /* # vertex pts to be read in */
int npe; /* # planes from 3 vertex points */
int neq; /* # planes from equation */
int nae; /* # planes from az, el & vertex index */
int nface; /* total number of faces */
double *input_points = (double *)0;
double *vertex = (double *)0; /* vertex list of final solid */
int last_vertex; /* index of first unused vertex */
int max_vertex; /* size of vertex array */
int *used = (int *)0; /* plane eqn use count */
plane_t *eqn = (plane_t *)0; /* plane equations */
int cur_eq = 0; /* current (free) equation number */
int symm = 0; /* symmetry about Y used */
register int i;
int j;
int k;
register int m;
point_t cent; /* centroid of arbn */
struct bn_tol tol;
/* XXX The tolerance here is sheer guesswork */
tol.magic = BN_TOL_MAGIC;
tol.dist = 0.005;
tol.dist_sq = tol.dist * tol.dist;
tol.perp = 1e-6;
tol.para = 1 - tol.perp;
npt = getint( scard, 10+0*10, 10 );
npe = getint( scard, 10+1*10, 10 );
neq = getint( scard, 10+2*10, 10 );
nae = getint( scard, 10+3*10, 10 );
nface = npe + neq + nae;
if ( npt < 1 ) {
/* Having one point is necessary to compute centroid */
printf("arbn defined without at least one point\n");
bad:
if (npt>0) eat( (npt+1)/2 ); /* vertex input_points */
if (npe>0) eat( (npe+5)/6 ); /* vertex pt index numbers */
if (neq>0) eat( neq ); /* plane eqns? */
if (nae>0) eat( (nae+1)/2 ); /* az el & vertex index? */
return(-1);
}
/* Allocate storage for plane equations */
eqn = (plane_t *)bu_malloc(nface*sizeof(plane_t), "eqn");
/* Allocate storage for per-plane use count */
used = (int *)bu_malloc(nface*sizeof(int), "used");
if ( npt >= 1 ) {
/* Obtain vertex input_points */
input_points = (double *)bu_malloc(npt*3*sizeof(double), "input_points");
if ( getxsoldata( input_points, npt*3, sol_work ) < 0 )
goto bad;
}
/* Get planes defined by three points, 6 per card */
for ( i=0; i<npe; i += 6 ) {
if ( get_line( scard, sizeof(scard), "arbn vertex point indices" ) == EOF ) {
printf("too few cards for arbn %d\n",
sol_work);
return(-1);
}
for ( j=0; j<6; j++ ) {
int q, r, s;
point_t a, b, c;
q = getint( scard, 10+j*10+0, 4 );
r = getint( scard, 10+j*10+4, 3 );
s = getint( scard, 10+j*10+7, 3 );
if ( q == 0 || r == 0 || s == 0 ) continue;
if ( q < 0 ) {
VMOVE( a, &input_points[((-q)-1)*3] );
a[Y] = -a[Y];
symm = 1;
} else {
VMOVE( a, &input_points[((q)-1)*3] );
}
if ( r < 0 ) {
VMOVE( b, &input_points[((-r)-1)*3] );
b[Y] = -b[Y];
symm = 1;
} else {
VMOVE( b, &input_points[((r)-1)*3] );
}
if ( s < 0 ) {
VMOVE( c, &input_points[((-s)-1)*3] );
c[Y] = -c[Y];
symm = 1;
} else {
VMOVE( c, &input_points[((s)-1)*3] );
}
if ( bn_mk_plane_3pts( eqn[cur_eq], a, b, c, &tol ) < 0 ) {
printf("arbn degenerate plane\n");
VPRINT("a", a);
VPRINT("b", b);
VPRINT("c", c);
continue;
}
cur_eq++;
}
}
/* Get planes defined by their equation */
for ( i=0; i < neq; i++ ) {
register double scale;
if ( get_line( scard, sizeof(scard), "arbn plane equation card" ) == EOF ) {
printf("too few cards for arbn %d\n",
sol_work);
return(-1);
}
eqn[cur_eq][0] = getdouble( scard, 10+0*10, 10 );
eqn[cur_eq][1] = getdouble( scard, 10+1*10, 10 );
eqn[cur_eq][2] = getdouble( scard, 10+2*10, 10 );
eqn[cur_eq][3] = getdouble( scard, 10+3*10, 10 );
scale = MAGNITUDE(eqn[cur_eq]);
if ( scale < SMALL ) {
printf("arbn plane normal too small\n");
continue;
}
scale = 1/scale;
VSCALE( eqn[cur_eq], eqn[cur_eq], scale );
eqn[cur_eq][3] *= scale;
cur_eq++;
}
/* Get planes defined by azimuth, elevation, and pt, 2 per card */
for ( i=0; i < nae; i += 2 ) {
if ( get_line( scard, sizeof(scard), "arbn az/el card" ) == EOF ) {
printf("too few cards for arbn %d\n",
sol_work);
return(-1);
}
for ( j=0; j<2; j++ ) {
double az, el;
int vert_no;
double cos_el;
point_t pt;
az = getdouble( scard, 10+j*30+0*10, 10 ) * bn_degtorad;
el = getdouble( scard, 10+j*30+1*10, 10 ) * bn_degtorad;
vert_no = getint( scard, 10+j*30+2*10, 10 );
if ( vert_no == 0 ) break;
cos_el = cos(el);
eqn[cur_eq][X] = cos(az)*cos_el;
eqn[cur_eq][Y] = sin(az)*cos_el;
eqn[cur_eq][Z] = sin(el);
if ( vert_no < 0 ) {
VMOVE( pt, &input_points[((-vert_no)-1)*3] );
pt[Y] = -pt[Y];
} else {
VMOVE( pt, &input_points[((vert_no)-1)*3] );
}
eqn[cur_eq][3] = VDOT(pt, eqn[cur_eq]);
cur_eq++;
}
}
if ( nface != cur_eq ) {
printf("arbn expected %d faces, got %d\n", nface, cur_eq);
return(-1);
}
/* Average all given points together to find centroid */
/* This is why there must be at least one (two?) point given */
VSETALL(cent, 0);
for ( i=0; i<npt; i++ ) {
VADD2( cent, cent, &input_points[i*3] );
}
VSCALE( cent, cent, 1.0/npt );
if ( symm ) cent[Y] = 0;
/* Point normals away from centroid */
for ( i=0; i<nface; i++ ) {
double dist;
dist = VDOT( eqn[i], cent ) - eqn[i][3];
/* If dist is negative, 'cent' is inside halfspace */
#define DIST_TOL (1.0e-8)
#define DIST_TOL_SQ (1.0e-10)
if ( dist < -DIST_TOL ) continue;
if ( dist > DIST_TOL ) {
/* Flip halfspace over */
VREVERSE( eqn[i], eqn[i] );
eqn[i][3] = -eqn[i][3];
} else {
/* Centroid lies on this face */
printf("arbn centroid lies on face\n");
return(-1);
}
}
/* Release storage for input points */
bu_free( (char *)input_points, "input_points" );
input_points = (double *)0;
/*
* ARBN must be convex. Test for concavity.
* Byproduct is an enumeration of all the verticies.
*/
last_vertex = max_vertex = 0;
/* Zero face use counts */
for ( i=0; i<nface; i++ ) {
used[i] = 0;
}
for ( i=0; i<nface-2; i++ ) {
for ( j=i+1; j<nface-1; j++ ) {
double dot;
int point_count; /* # points on this line */
/* If normals are parallel, no intersection */
dot = VDOT( eqn[i], eqn[j] );
if ( !NEAR_ZERO( dot, 0.999999 ) ) continue;
point_count = 0;
for ( k=j+1; k<nface; k++ ) {
point_t pt;
if ( bn_mkpoint_3planes( pt, eqn[i], eqn[j], eqn[k] ) < 0 ) continue;
/* See if point is outside arb */
for ( m=0; m<nface; m++ ) {
if ( i==m || j==m || k==m ) continue;
if ( VDOT(pt, eqn[m])-eqn[m][3] > DIST_TOL )
goto next_k;
}
/* See if vertex already was found */
for ( m=0; m<last_vertex; m++ ) {
vect_t dist;
VSUB2( dist, pt, &vertex[m*3] );
if ( MAGSQ(dist) < DIST_TOL_SQ )
goto next_k;
}
/*
* Add point to vertex array.
* If more room needed, realloc.
*/
if ( last_vertex >= max_vertex ) {
if ( max_vertex == 0 ) {
max_vertex = 3;
vertex = (double *)bu_malloc( max_vertex*3*sizeof(double), "vertex" );
} else {
max_vertex *= 10;
vertex = (double *)bu_realloc( (char *)vertex, max_vertex*3*sizeof(double), "vertex" );
}
}
VMOVE( &vertex[last_vertex*3], pt );
last_vertex++;
point_count++;
/* Increment "face used" counts */
used[i]++;
used[j]++;
used[k]++;
next_k:
;
}
if ( point_count > 2 ) {
printf("arbn: warning, point_count on line=%d\n", point_count);
}
}
}
/* If any planes were not used, then arbn is not convex */
for ( i=0; i<nface; i++ ) {
if ( used[i] != 0 ) continue; /* face was used */
printf("arbn face %d unused, solid is not convex\n", i);
return(-1);
}
/* Write out the solid ! */
i = mk_arbn( outfp, name, nface, eqn );
if ( input_points ) bu_free( (char *)input_points, "input_points" );
if ( vertex ) bu_free( (char *)vertex, "vertex" );
if ( eqn ) bu_free( (char *)eqn, "eqn" );
if ( used ) bu_free( (char *)used, "used" );
return(i);
}
int main() {
for (n = 0; n < SIZE && (getdouble(&array[n])) != EOF; n++)
printf("%F ", array[n]);
return 0;
}
void init_ugraph(graph_t *g)
{
char *p;
double xf;
static char *rankname[] = {"local","global","none",NULL};
static int rankcode[] = {LOCAL, GLOBAL, NOCLUST, LOCAL};
double X,Y,Z,x,y;
GD_drawing(g) = NEW(layout_t);
/* set this up fairly early in case any string sizes are needed */
if ((p = agget(g,"fontpath")) || (p = getenv("DOTFONTPATH"))) {
/* overide GDFONTPATH in local environment if dot
* wants its own */
#ifdef HAVE_SETENV
setenv("GDFONTPATH", p, 1);
#else
static char *buf=0;
buf=grealloc(buf,strlen("GDFONTPATH=")+strlen(p)+1);
strcpy(buf,"GDFONTPATH=");
strcat(buf,p);
putenv(buf);
#endif
}
GD_drawing(g)->quantum = late_double(g,agfindattr(g,"quantum"),0.0,0.0);
GD_drawing(g)->font_scale_adj = 1.0;
/* setting rankdir=LR is only defined in dot,
* but having it set causes shape code and others to use it.
* The result is confused output, so we turn it off unless requested.
*/
if (UseRankdir)
GD_left_to_right(g) = ((p = agget(g,"rankdir")) && streq(p,"LR"));
else
GD_left_to_right(g) = FALSE;
do_graph_label(g);
xf = late_double(g,agfindattr(g,"nodesep"),DEFAULT_NODESEP,MIN_NODESEP);
GD_nodesep(g) = POINTS(xf);
p = late_string(g,agfindattr(g,"ranksep"),NULL);
if (p) {
if (sscanf(p,"%lf",&xf) == 0) xf = DEFAULT_RANKSEP;
else {if (xf < MIN_RANKSEP) xf = MIN_RANKSEP;}
if (strstr(p,"equally")) GD_exact_ranksep(g) = TRUE;
}
else xf = DEFAULT_RANKSEP;
GD_ranksep(g) = POINTS(xf);
GD_showboxes(g) = late_int(g,agfindattr(g,"showboxes"),0,0);
Epsilon = .0001 * agnnodes(g);
getdoubles2pt(g,"size",&(GD_drawing(g)->size));
getdoubles2pt(g,"page",&(GD_drawing(g)->page));
getdouble(g,"epsilon",&Epsilon);
getdouble(g,"nodesep",&Nodesep);
getdouble(g,"nodefactor",&Nodefactor);
X = Y = Z = x = y = 0.0;
if ((p = agget(g,"viewport")))
sscanf(p,"%lf,%lf,%lf,%lf,%lf",&X,&Y,&Z,&x,&y);
GD_drawing(g)->viewport.size.x = ROUND(X); /* viewport size in dev units - pixels */
GD_drawing(g)->viewport.size.y = ROUND(Y);
GD_drawing(g)->viewport.zoom = Z; /* scaling factor */
GD_drawing(g)->viewport.focus.x = x; /* graph coord of focus - points */
GD_drawing(g)->viewport.focus.y = y;
GD_drawing(g)->centered = mapbool(agget(g,"center"));
if ((p = agget(g,"rotate"))) GD_drawing(g)->landscape = (atoi(p) == 90);
else { /* today we learned the importance of backward compatibilty */
if ((p = agget(g,"orientation")))
GD_drawing(g)->landscape = ((p[0] == 'l') || (p[0] == 'L'));
}
p = agget(g,"clusterrank");
CL_type = maptoken(p,rankname,rankcode);
p = agget(g,"concentrate");
Concentrate = mapbool(p);
Nodesep = 1.0;
Nodefactor = 1.0;
Initial_dist = MYHUGE;
}
int main(int argc, char *argv[])
{
char *fmt, *start;
int fieldwidth, precision;
char nextch;
char *format;
int ch;
int error;
#if !defined(SHELL) && !defined(BUILTIN)
(void)setlocale (LC_ALL, "");
#endif
while ((ch = getopt(argc, argv, "")) != -1) {
switch (ch) {
case '?':
default:
usage();
return 1;
}
}
argc -= optind;
argv += optind;
if (argc < 1) {
usage();
return 1;
}
format = *argv;
gargv = ++argv;
#define SKIP1 "#-+ 0"
#define SKIP2 "0123456789"
do {
/*
* Basic algorithm is to scan the format string for conversion
* specifications -- once one is found, find out if the field
* width or precision is a '*'; if it is, gather up value.
* Note, format strings are reused as necessary to use up the
* provided arguments, arguments of zero/null string are
* provided to use up the format string.
*/
/* find next format specification */
for (fmt = format; (ch = *fmt++) != '\0';) {
if (ch == '\\') {
char c_ch;
fmt = conv_escape(fmt, &c_ch);
putchar(c_ch);
continue;
}
if (ch != '%' || (*fmt == '%' && ++fmt)) {
(void)putchar(ch);
continue;
}
/* Ok - we've found a format specification,
Save its address for a later printf(). */
start = fmt - 1;
/* skip to field width */
fmt += strspn(fmt, SKIP1);
if (*fmt == '*') {
fmt++;
fieldwidth = getwidth();
} else
fieldwidth = -1;
/* skip to possible '.', get following precision */
fmt += strspn(fmt, SKIP2);
if (*fmt == '.') {
fmt++;
if (*fmt == '*') {
fmt++;
precision = getwidth();
} else
precision = -1;
} else
precision = -1;
fmt += strspn(fmt, SKIP2);
ch = *fmt;
if (!ch) {
warnx("missing format character");
return (1);
}
/* null terminate format string to we can use it
as an argument to printf. */
nextch = fmt[1];
fmt[1] = 0;
switch (ch) {
case 'B': {
const char *p = conv_expand(getstr());
if (p == NULL)
goto out;
*fmt = 's';
PF(start, p);
if (error < 0)
goto out;
break;
}
case 'b': {
/* There has to be a better way to do this,
* but the string we generate might have
* embedded nulls. */
static char *a, *t;
char *cp = getstr();
/* Free on entry in case shell longjumped out */
if (a != NULL)
free(a);
a = NULL;
if (t != NULL)
free(t);
t = NULL;
/* Count number of bytes we want to output */
b_length = 0;
conv_escape_str(cp, b_count);
t = malloc(b_length + 1);
if (t == NULL)
goto out;
(void)memset(t, 'x', b_length);
t[b_length] = 0;
/* Get printf to calculate the lengths */
*fmt = 's';
abort();
/* APF(&a, start, t); */
if (error == -1)
goto out;
b_fmt = a;
/* Output leading spaces and data bytes */
conv_escape_str(cp, b_output);
/* Add any trailing spaces */
printf("%s", b_fmt);
break;
}
case 'c': {
char p = getchr();
PF(start, p);
if (error < 0)
goto out;
break;
}
case 's': {
char *p = getstr();
PF(start, p);
if (error < 0)
goto out;
break;
}
case 'd':
case 'i': {
intmax_t p = getintmax();
char *f = mklong(start, ch);
PF(f, p);
if (error < 0)
goto out;
break;
}
case 'o':
case 'u':
case 'x':
case 'X': {
uintmax_t p = getuintmax();
char *f = mklong(start, ch);
PF(f, p);
if (error < 0)
goto out;
break;
}
case 'e':
case 'E':
case 'f':
case 'g':
case 'G': {
double p = getdouble();
PF(start, p);
if (error < 0)
goto out;
break;
}
default:
warnx("%s: invalid directive", start);
return 1;
}
*fmt++ = ch;
*fmt = nextch;
/* escape if a \c was encountered */
if (rval & 0x100)
return rval & ~0x100;
}
} while (gargv != argv && *gargv);
return rval & ~0x100;
out:
warn("print failed");
return 1;
}
/*
* D O F I L E
*/
void
dofile(FILE *fp)
{
register int c;
while ( (c = getc(fp)) != EOF ) {
switch ( c ) {
/* One of a kind functions */
case 'e':
case 'F':
putchar( c );
break;
case 'f':
case 't':
putchar( c );
copy_string( fp );
break;
case 'C':
putchar( c );
COPY(3);
break;
case 'c': /* map x, y? */
putchar( c );
COPY(6);
break;
case 'a': /* map points? */
putchar( c );
COPY(12);
break;
case 'p':
case 'm':
case 'n':
/* Two coordinates in, three out. Change command. */
putchar( UPPER_CASE(c) );
two_coord_out( fp, rmat );
break;
case 'l':
putchar( 'L' );
two_coord_out( fp, rmat );
two_coord_out( fp, rmat );
break;
case 'P':
case 'M':
case 'N':
putchar( c );
three_coord_out( fp, rmat );
break;
case 'L':
putchar( c );
three_coord_out( fp, rmat );
three_coord_out( fp, rmat );
break;
case 's':
{
/* 2-D integer SPACE command.
* This is the only AT&T compatible space
* command; be certain to only output
* with pl_space(), to ensure that output
* file is AT&T style if input was.
*/
long minx, miny, maxx, maxy;
point_t min, max;
minx = getshort(fp);
miny = getshort(fp);
maxx = getshort(fp);
maxy = getshort(fp);
VSET( min, minx, miny, -1 );
VSET( max, maxx, maxy, -1 );
model_rpp( min, max );
minx = (long)floor( space_min[X] );
miny = (long)floor( space_min[Y] );
maxx = (long)ceil( space_max[X] );
maxy = (long)ceil( space_max[Y] );
if ( minx < -32768 ) minx = -32768;
if ( miny < -32768 ) miny = -32768;
if ( maxx > 32767 ) maxx = 32767;
if ( maxy > 32767 ) maxy = 32767;
pl_space( stdout, minx, miny, maxx, maxy );
}
break;
case 'S':
{
/* BRL-extended 3-D integer SPACE command */
point_t min, max;
min[X] = getshort(fp);
min[Y] = getshort(fp);
min[Z] = getshort(fp);
max[X] = getshort(fp);
max[Y] = getshort(fp);
max[Z] = getshort(fp);
model_rpp( min, max );
pdv_3space( stdout, space_min, space_max );
}
break;
/* 2D and 3D IEEE */
case 'w':
{
/* BRL 2-D floating point SPACE command */
point_t min, max;
min[X] = getdouble(fp);
min[Y] = getdouble(fp);
min[Z] = -1.0;
max[X] = getdouble(fp);
max[Y] = getdouble(fp);
max[Z] = 1.0;
model_rpp( min, max );
pdv_3space( stdout, space_min, space_max );
}
break;
case 'W':
{
/* BRL 3-D floating point SPACE command */
point_t min, max;
min[X] = getdouble(fp);
min[Y] = getdouble(fp);
min[Z] = getdouble(fp);
max[X] = getdouble(fp);
max[Y] = getdouble(fp);
max[Z] = getdouble(fp);
model_rpp( min, max );
pdv_3space( stdout, space_min, space_max );
}
break;
case 'i':
putchar(c);
COPY(3*8);
break;
case 'r':
putchar(c);
COPY(6*8);
break;
case 'x':
case 'o':
case 'q':
/* Two coordinates in, three out. Change command. */
putchar( UPPER_CASE(c) );
two_dcoord_out( fp, rmat );
break;
case 'v':
/* Two coordinates in, three out. Change command. */
putchar( 'V' );
two_dcoord_out( fp, rmat );
two_dcoord_out( fp, rmat );
break;
case 'X':
case 'O':
case 'Q':
putchar( c );
three_dcoord_out( fp, rmat );
break;
case 'V':
putchar( c );
three_dcoord_out( fp, rmat );
three_dcoord_out( fp, rmat );
break;
default:
fprintf( stderr, "plrot: unrecognized command '%c' (0x%x)\n",
(isascii(c) && isprint(c)) ? c : '?',
c );
fprintf( stderr, "plrot: ftell = %ld\n", ftell(fp) );
putchar( c );
break;
}
}
}
double VariantContainer<T>::ToDouble(void) const
{
return getdouble( );
}
main(int argc, char *argv[])
#endif
{
char *fmt, *start;
int fieldwidth, precision;
char convch, nextch;
char *format;
#if !defined(SHELL) && !defined(BUILTIN)
setlocale (LC_ALL, "");
#endif
/* Need to accept/ignore "--" option. */
if (argc > 1 && strcmp(argv[1], "--") == 0) {
argc--;
argv++;
}
if (argc < 2) {
usage();
return (1);
}
format = *++argv;
gargv = ++argv;
#define SKIP1 "#-+ 0"
#define SKIP2 "*0123456789"
do {
/*
* Basic algorithm is to scan the format string for conversion
* specifications -- once one is found, find out if the field
* width or precision is a '*'; if it is, gather up value.
* Note, format strings are reused as necessary to use up the
* provided arguments, arguments of zero/null string are
* provided to use up the format string.
*/
/* find next format specification */
for (fmt = format; *fmt; fmt++) {
switch (*fmt) {
case '%':
start = fmt++;
if (*fmt == '%') {
putchar ('%');
break;
} else if (*fmt == 'b') {
char *p = getstr();
if (print_escape_str(p)) {
return (rval);
}
break;
}
/* skip to field width */
for (; strchr(SKIP1, *fmt); ++fmt) ;
fieldwidth = *fmt == '*' ? getint() : 0;
/* skip to possible '.', get following precision */
for (; strchr(SKIP2, *fmt); ++fmt) ;
if (*fmt == '.')
++fmt;
precision = *fmt == '*' ? getint() : 0;
for (; strchr(SKIP2, *fmt); ++fmt) ;
if (!*fmt) {
warnx ("missing format character");
return(1);
}
convch = *fmt;
nextch = *(fmt + 1);
*(fmt + 1) = '\0';
switch(convch) {
case 'c': {
char p = getchr();
PF(start, p);
break;
}
case 's': {
char *p = getstr();
PF(start, p);
break;
}
case 'd':
case 'i': {
long p;
char *f = mklong(start, convch);
if (!f) {
warnx("out of memory");
return (1);
}
p = getlong();
PF(f, p);
break;
}
case 'o':
case 'u':
case 'x':
case 'X': {
unsigned long p;
char *f = mklong(start, convch);
if (!f) {
warnx("out of memory");
return (1);
}
p = getulong();
PF(f, p);
break;
}
case 'e':
case 'E':
case 'f':
case 'g':
case 'G': {
double p = getdouble();
PF(start, p);
break;
}
default:
warnx ("%s: invalid directive", start);
return(1);
}
*(fmt + 1) = nextch;
break;
case '\\':
fmt += print_escape(fmt);
break;
default:
putchar (*fmt);
break;
}
}
} while (gargv > argv && *gargv);
return (rval);
}
