void reset_vel(PARAMS *p,double f,SSDATA *d)
{
VECTOR r_hat,vr_vec,t_hat,vt_vec;
double v,vr,vt;
/* set speed scaled by desired virial fraction */
SCALE_VEC(d->vel,sqrt(f));
/* adjust radial and tangential components as desired */
if (p->radf < 0) return;
COPY_VEC(d->pos,r_hat);
NORM_VEC(r_hat,MAG(r_hat)); /* radial unit vector */
vr = DOT(d->vel,r_hat);
COPY_VEC(r_hat,vr_vec);
SCALE_VEC(vr_vec,vr); /* current radial component of vel */
SUB_VEC(d->vel,vr_vec,t_hat);
NORM_VEC(t_hat,MAG(t_hat)); /* tangential unit vector */
v = MAG(d->vel);
vr = sqrt(p->radf)*v;
vt = sqrt(1 - p->radf)*v;
COPY_VEC(r_hat,vr_vec);
SCALE_VEC(vr_vec,vr); /* new radial component */
COPY_VEC(t_hat,vt_vec);
SCALE_VEC(vt_vec,vt); /* new tangential component */
ADD_VEC(vr_vec,vt_vec,d->vel);
}
int
main(int argc,char *argv[])
{
/* rotates input vector by specified angle around given rotation axis */
VECTOR r,n,nxr,rp;
double phi,cphi,sphi,ndotr;
if (argc != 8) {
(void) fprintf(stderr,"Usage: %s x y z phi rx ry rz\n",argv[0]);
return 1;
}
SET_VEC(r,atof(argv[1]),atof(argv[2]),atof(argv[3]));
assert(MAG(r) > 0.0);
phi = atof(argv[4]);
SET_VEC(n,atof(argv[5]),atof(argv[6]),atof(argv[7]));
assert(MAG(n) > 0.0);
ndotr = DOT(n,r);
CROSS(n,r,nxr);
cphi = cos(phi);
sphi = sin(phi);
SCALE_VEC(r,cphi);
SCALE_VEC(n,ndotr*(1.0-cphi));
SCALE_VEC(nxr,sphi);
ADD_VEC(r,n,rp);
ADD_VEC(rp,nxr,rp);
(void) printf("%g %g %g\n",rp[X],rp[Y],rp[Z]);
return 0;
}
static double SELECT_TIME_STEP(struct pqNode *p)
{
double eta = DFLT_ETA;
double v = MAG(VEL(p));
double T = 0.5*MASS(p)*v*v;
return 0.25 * eta*eta / (T - p->U);
}
void gen_body(PARAMS *p,SSDATA *d,int iOrgIdx)
{
double phi,theta,v;
int k;
d->mass = p->m;
d->radius = p->R;
for (k=0;k<N_DIM;k++) {
do {
SET_VEC(d->pos,(2*RAN - 1)*p->Rd,(2*RAN - 1)*p->Rd,(2*RAN - 1)*p->Rd);
} while (MAG(d->pos) + p->R > p->Rd);
/* use following for more centrally condensed distribution...
r = RAN*(p->Rd - p->R);
phi = TWO_PI*RAN;
theta = PI*RAN;
SET_VEC(d->pos,sin(theta)*cos(phi),sin(theta)*sin(phi),cos(theta));
SCALE_VEC(d->pos,r);
*/
v = RAN; /* will be rescaled later */
phi = TWO_PI*RAN;
theta = PI*RAN;
SET_VEC(d->vel,sin(theta)*cos(phi),sin(theta)*sin(phi),cos(theta));
SCALE_VEC(d->vel,v); /* isotropic initially */
}
ZERO_VEC(d->spin);
d->color = PLANETESIMAL;
d->org_idx = iOrgIdx;
}
static inline void init_ray_computer (const camera_t *camera)
{
if (!inited) {
vector_t right, down;
float mag;
inited = 1;
mag = MAG (camera->right);
/* find the left top corner of the screen */
ADD (left_top, camera->location, camera->direction);
SUB (left_top, left_top, camera->right);
ADD (left_top, left_top, camera->up);
SMUL (right, 2, camera->right);
SMUL (down, -2, camera->up);
/* find the vector corresponding to a movement to the left & right */
mag = 1.0 / (bp_scene_get_horizontal_resolution (curr_scene) - 1);
SMUL (right_step, mag, right);
mag = 1.0 / (bp_scene_get_vertical_resolution (curr_scene) - 1);
SMUL (down_step, mag, down);
}
}
void
bp_camera_set_look_at (camera_t *camera, vector_t look_at)
{
float dir_length, up_length, right_length;
vector_t dir; /* direction vector not normalized */
dir_length = MAG (camera->direction);
up_length = MAG (camera->up);
right_length = MAG (camera->right);
SUB (dir, look_at, camera->location);
CROSS (camera->right, dir, camera->sky);
VRESIZE (camera->right, right_length);
CROSS (camera->up, camera->right, dir);
VRESIZE (camera->up, up_length);
VSET_SIZE (camera->direction, dir, dir_length);
}
void
bp_camera_set_angle (camera_t *camera, float angle)
{
float direction_length;
if (angle >= 180)
angle = 179;
direction_length = 0.5 * MAG (camera->right) / tan (angle / 360 * M_PI);
VRESIZE (camera->direction, direction_length);
camera->angle = angle;
}
void MakeBasis(VECTOR v1, VECTOR v2, VECTOR v3)
{
/*
* Uses the Gram-Schmidt process to convert the spanning set v1, v2, v3
* into an orthonormal basis set.
*
*/
VECTOR n, v;
double proj;
/* Construct first basis vector */
NORM_VEC(v1, MAG(v1));
/* Construct second basis vector */
COPY_VEC(v1, n);
proj = DOT(v2, n);
SCALE_VEC(n, proj);
SUB_VEC(v2, n, v2);
NORM_VEC(v2, MAG(v2));
/* Construct third basis vector */
COPY_VEC(v3, v);
COPY_VEC(v2, n);
proj = DOT(v, n);
SCALE_VEC(n, proj);
SUB_VEC(v3, n, v3);
COPY_VEC(v1, n);
proj = DOT(v, n);
SCALE_VEC(n, proj);
SUB_VEC(v3, n, v3);
NORM_VEC(v3, MAG(v3));
}
/** @brief CheckMagForMBSet
*
* @todo: document this function
*/
int MandelBrotThread::CheckMagForMBSet(double aVal, double bVal)
{
double x = 0.0;
double y = 0.0;
double xTemp = 0.0;
int iterateVal = 0;
do
{
xTemp = RZn( x, y, aVal);
y = iZn( x, y, bVal);
x = xTemp;
if(iterateVal ++ > ITERATION)
{
break;
}
}while( MAG(x,y) <= ( (MAX_MAG) * (MAX_MAG) ) );
return iterateVal;
}
int main(){
int i = 0;
int mapCount = 0, clearMapCount = 0, dumpCount=0;
int revFrameCount = 0;
#ifdef USE_NORTH
targetsGPS[maxTargets].lat = ADVANCED5LAT;
targetsGPS[maxTargets].lon = ADVANCED5LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED6LAT;
targetsGPS[maxTargets].lon = ADVANCED6LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED7LAT;
targetsGPS[maxTargets].lon = ADVANCED7LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED8LAT;
targetsGPS[maxTargets].lon = ADVANCED8LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED2LAT;
targetsGPS[maxTargets].lon = ADVANCED2LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED1LAT;
targetsGPS[maxTargets].lon = ADVANCED1LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED3LAT;
targetsGPS[maxTargets].lon = ADVANCED3LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED12LAT;
targetsGPS[maxTargets].lon = ADVANCED12LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED4LAT;
targetsGPS[maxTargets].lon = ADVANCED4LON;
maxTargets++;
#else
targetsGPS[maxTargets].lat = ADVANCED4LAT;
targetsGPS[maxTargets].lon = ADVANCED4LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED1LAT;
targetsGPS[maxTargets].lon = ADVANCED1LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED2LAT;
targetsGPS[maxTargets].lon = ADVANCED2LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED3LAT;
targetsGPS[maxTargets].lon = ADVANCED3LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED11LAT;
targetsGPS[maxTargets].lon = ADVANCED11LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED8LAT;
targetsGPS[maxTargets].lon = ADVANCED8LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED7LAT;
targetsGPS[maxTargets].lon = ADVANCED7LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED6LAT;
targetsGPS[maxTargets].lon = ADVANCED6LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED11LAT;
targetsGPS[maxTargets].lon = ADVANCED11LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED5LAT;
targetsGPS[maxTargets].lon = ADVANCED5LON;
maxTargets++;
#endif
maxTargetIndex=maxTargets-1;
for(i=0;i<maxTargets;i++){// this is converting all GPS point data to XY data.
targetListXY[i].x = GPSX(targetsGPS[i].lon, startLongitude);
targetListXY[i].y = GPSY(targetsGPS[i].lat, startLatitude);
}
currentXY.x = GPSX(gpsvar.longitude,startLongitude);// converts current robot X location compared to start longitude
currentXY.y = GPSY(gpsvar.latitude,startLatitude);// converts current robot Y location compared to start latitude
targetXY = targetListXY[currentTargetIndex];//sets first target GPS point
nextTargetIndex = (currentTargetIndex + 1)%maxTargets;//sets next target GPS point
nextXY = targetListXY[nextTargetIndex];// ??
previousXY.x = GPSX(startLongitude, startLongitude);// why?
previousXY.y = GPSY(startLatitude, startLatitude);//Why?
initRoboteq(); /* Initialize roboteq */
initGuide();//what is guide?
#ifdef USE_VISION // if USE_vision is defined, then initialize vision.
initVision();
#endif //USE_VISION
#ifdef USE_GPS// if USE_GPS is defined, then initialize GPS.
initGPS();
initParser();
#endif //USE_GPS
#ifdef USE_LIDAR// if USE_LIDAR is defined, then initialize LIDAR.
initObjects();
initSICK();
#endif //USE_LIDAR
#ifdef DEBUG_VISUALIZER// if defined, then use visualizer.
initVisualizer();
#endif //DEBUG_VISUALIZER
#ifdef USE_MAP//////>>>>>>>>>>>????
initMap(0,0,0);
#endif //USE_MAP
#ifdef DUMP_GPS// dump GPS data into file
FILE *fp;
fp = fopen("gpsdump.txt", "w");
#endif // DUMP_GPS
while(1){
double dir = 1.0;
double speed = 0.0, turn = 0.0;
static double turnBoost = 0.750;//Multiplier for turn. Adjust to smooth jerky motions. Usually < 1.0
static int lSpeed = 0, rSpeed = 0;//Wheel Speed Variables
if (joystick() != 0) {// is joystick is connected
if (joy0.buttons & LB_BTN) {// deadman switch, but what does joy0.buttons do?????????????????????????????????
speed = -joy0.axis[1]; //Up is negative on joystick negate so positive when going forward
turn = joy0.axis[0];
lSpeed = (int)((speed + turnBoost*turn)*maxSpeed);//send left motor speed
rSpeed = (int)((speed - turnBoost*turn)*maxSpeed);//send right motor speed
}else{ //stop the robot
rSpeed=lSpeed=0;
}
if(((joy0.buttons & B_BTN)||autoOn)&& (saveImage==0)){//what is the single & ???????????????????
saveImage =DEBOUNCE_FOR_SAVE_IMAGE;//save each image the camera takes, save image is an int declared in vision_nav.h
}else{
if (saveImage) saveImage--; // turn off if button wasn't pressed?
}
if(joy0.buttons & RB_BTN){//turn on autonmous mode if start??? button is pressed
autoOn = 1;
mode=1;
}
if(joy0.buttons & Y_BTN){ // turn off autonomous mode
autoOn = 0;
mode =0;
}
lastButtons = joy0.buttons;//is this just updating buttons?
} else{
// printf("No Joystick Found!\n");
rSpeed=lSpeed=0;
}
//
// printf("3: %f %f\n",BASIC3LAT,BASIC3LON);
// printf("4: %f %f\n",BASIC4LAT,BASIC4LON);
// printf("5: %f %f\n",BASIC5LAT,BASIC5LON);
// getchar();
#ifdef AUTO_SWAP//what is this
if((currentTargetIndex>1&&targetIndexMem!=currentTargetIndex)||!autoOn||!mode==3){
startTime=currentTime=(float)(clock()/CLOCKS_PER_SEC);
targetIndexMem = currentTargetIndex;
}else{
currentTime=(float)(clock()/CLOCKS_PER_SEC);
}
totalTime = currentTime-startTime;
if(totalTime>=SWAPTIME&&autoOn){
swap();
targetIndexMem = 0;
}
#endif //AUTO_SWAP
#ifdef USE_GPS
readGPS();
currentXY.x = GPSX(gpsvar.longitude,startLongitude);
currentXY.y = GPSY(gpsvar.latitude,startLatitude);
robotTheta = ADJUST_RADIANS(DEG2RAD(gpsvar.course));
#else
currentXY.x = 0.0;
currentXY.y = 0.0;
robotTheta = 0.0;
#endif //USE_GPS
if(autoOn&&!flagPointSet){//this whole thing?????
flagXY.x=currentXY.x+FLAG_X_ADJUST;
flagXY.y=currentXY.y;
flagPointSet=1;
startAutoTime=currentAutoTime=(float)(clock()/CLOCKS_PER_SEC);
}
if(autoOn){
currentAutoTime=(float)(clock()/CLOCKS_PER_SEC);
totalAutoTime = currentAutoTime-startAutoTime;
if(totalAutoTime>=MODE2DELAY){
mode1TimeUp=1;//what is mode1 time up?
}
printf("TIMEING\n");
}
// if(currentTargetIndex <= OPEN_FIELD_INDEX || currentTargetIndex >= maxTargetIndex){
if(currentTargetIndex <= OPEN_FIELD_INDEX){//if you are on your last target, then set approaching thresh, and dest thresh to larger values?
//OPEN_FIELD_INDEX is set to 0 above...?
approachingThresh=4.0;
destinationThresh=3.0;
}else{//otherwise set your thresholds to a bit closer.
// destinationThresh=1.0;
destinationThresh=0.75;
approachingThresh=2.5;
}
//mode1 = lane tracking and obstacle avoidance. mode 2 = vision, lane tracking, but guide to gps. its not primary focus.
//mode3= gps mode in open field, but vision is toned down to not get distracted by random grass.
//mode 4= flag tracking
if(guide(currentXY, targetXY, previousXY, nextXY, robotTheta, robotWidth, 1)&& !allTargetsReached){//If target reached and and not all targets reached
printf("REACHED TARGET\n");
initGuide();// reset PID control stuff. problably resets all control variables.
previousXY = targetXY;//update last target
if(currentTargetIndex == maxTargetIndex){ //seeing if you are done with all targets.
allTargetsReached = 1;
}else{//otherwise update all the target information
currentTargetIndex = (currentTargetIndex + 1);
nextTargetIndex = (currentTargetIndex + 1)% maxTargets;
targetXY = targetListXY[currentTargetIndex];
nextXY = targetListXY[nextTargetIndex];
}
}
if((autoOn&&(currentTargetIndex == 0&&!approachingTarget&&!mode1TimeUp))||allTargetsReached){
//if autonomous, and on first target, and not not approaching target, and not mode 1 time up, or reached last target.
mode =1;//wtf is mode
distanceMultiplier = 50;//wthis is how heavily to rely on vision
} else if((autoOn&¤tTargetIndex == 0&&mode1TimeUp)||(autoOn&&approachingTarget&&(currentTargetIndex<=OPEN_FIELD_INDEX||currentTargetIndex>=maxTargetIndex-END_LANE_INDEX))){
mode =2;
distanceMultiplier = 50;
} else if((autoOn&¤tTargetIndex!=0)){
mode =3;
distanceMultiplier = 12;
}
flagPointDistance = D((currentXY.x-flagXY.x),(currentXY.y-flagXY.y));// basically the distance formula, but to what? what flags GPS point?
if(allTargetsReached&&flagPointDistance<FLAG_DIST_THRESH){
mode =4;// what is mode
}
#ifdef FLAG_TESTING
/*FLAG TESTING*/
mode=4;
#endif //FLAG_TESTING
/*Current Target Heading PID Control Adjustment*/
cvar.lookAhead = 0.00;//?
cvar.kP = 0.20; cvar.kI = 0.000; cvar.kD = 0.15;
turn = cvar.turn;
int bestVisGpsMask = 99;
int h = 0;
double minVisGpsTurn = 9999;
for(h=0;h<11;h++){
if(fabs((cvar.turn-turn_angle[h]))<minVisGpsTurn){
minVisGpsTurn=fabs((cvar.turn-turn_angle[h]));
bestVisGpsMask = h;
}
}
bestGpsMask = bestVisGpsMask;
// printf("bvg: %d \n", bestVisGpsMask);
// printf("vgt: %f cv3: %f\n", minVisGpsTurn,cvar3.turn);
#ifdef USE_VISION
// double visTurnBoost = 0.50;
double visTurnBoost = 1.0;
if(imageProc(mode) == -1) break;
if(mode==1||mode==2){
turn = turn_angle[bestmask];
turn *= visTurnBoost;
}else if(mode==3 && fabs(turn_angle[bestmask])>0.70){
turn = turn_angle[bestmask];
turn *= visTurnBoost;
}
#endif //USE_VISION
#ifdef USE_LIDAR
updateSick();
// findObjects();
#endif //USE_LIDAR
#ifdef USE_COMBINED_BUFFER//??????????
#define WORSTTHRESH 10
#define BESTTHRESH 3
if(mode==4){
#ifdef USE_NORTH
turn = (0.5*turn_angle[bestBlueMask]+0.5*turn_angle[bestRedMask]);
#else
turn = (0.65*turn_angle[bestBlueMask]+0.35*turn_angle[bestRedMask]);
#endif
turn *= 0.75;
}
combinedTargDist = cvar.targdist;
if(((approachingTarget||inLastTarget)&¤tTargetIndex>OPEN_FIELD_INDEX
&¤tTargetIndex<maxTargetIndex-END_LANE_INDEX)||(MAG(howbad[worstmask]-howbad[bestmask]))<BESTTHRESH||mode==4){
getCombinedBufferAngles(0,0);//Don't Use Vision Radar Data
}else{
getCombinedBufferAngles(0,1);//Use Vision Radar Data
}
if(combinedBufferAngles.left != 0 || combinedBufferAngles.right !=0){
if(mode == 1 || mode==2 || mode==3 || mode==4){
// if(mode == 1 || mode==2 || mode==3){
// if(mode==2 || mode==3){
// if(mode==3){
if(fabs(combinedBufferAngles.right)==fabs(combinedBufferAngles.left)){
double revTurn;
double revDistLeft, revDistRight;
int revIdx;
if(fabs(turn)<0.10) dir = -1.0;
if(fabs(combinedBufferAngles.left)>1.25) dir = -1.0;
if(dir<0){
revIdx = 540-RAD2DEG(combinedBufferAngles.left)*4;
revIdx = MIN(revIdx,1080);
revIdx = MAX(revIdx,0);
revDistLeft = LMSdata[revIdx];
revIdx = 540-RAD2DEG(combinedBufferAngles.right)*4;
revIdx = MIN(revIdx,1080);
revIdx = MAX(revIdx,0);
revDistRight = LMSdata[revIdx];
if(revDistLeft>=revDistRight){
revTurn = combinedBufferAngles.left;
}else {
revTurn = combinedBufferAngles.right;
}
turn = revTurn;
}else{
turn = turn_angle[bestmask];
}
} else if(fabs(combinedBufferAngles.right-turn)<fabs(combinedBufferAngles.left-turn)){
// } else if(turn<=0){
turn = combinedBufferAngles.right;
}else {
turn = combinedBufferAngles.left;
}
}
}
#endif //USE_COMBINED_BUFFER
if(dir<0||revFrameCount!=0){
dir = -1.0;
revFrameCount = (revFrameCount+1)%REVFRAMES;
}
// turn *= dir;
turn = SIGN(turn) * MIN(fabs(turn), 1.0);
speed = 1.0/(1.0+1.0*fabs(turn))*dir;
speed = SIGN(speed) * MIN(fabs(speed), 1.0);
if(!autoOn){
maxSpeed = 60;
targetIndexMem = 0;
}else if(dir<0){
maxSpeed = 30;
}else if(mode<=2||(mode==3 && fabs(turn_angle[bestmask])>0.25)){
maxSpeed = 60 - 25*fabs(turn);
// maxSpeed = 70 - 35*fabs(turn);
// maxSpeed = 90 - 50*fabs(turn);
// maxSpeed = 100 - 65*fabs(turn);
}else if(mode==4){
maxSpeed = 45-20*fabs(turn);
}else{
maxSpeed = 85 - 50*fabs(turn);
// maxSpeed = 100 - 65*fabs(turn);
// maxSpeed = 110 - 70*fabs(turn);
// maxSpeed = 120 - 85*fabs(turn);
}
if(autoOn){
lSpeed = (speed + turnBoost*turn) * maxSpeed;
rSpeed = (speed - turnBoost*turn) * maxSpeed;
}
#ifdef DEBUG_MAIN
printf("s:%.4f t: %.4f m: %d vt:%f dir:%f tmr: %f\n", speed, turn, mode, turn_angle[bestmask], flagPointDistance, totalAutoTime);
#endif //DEBUG_MAIN
#ifdef DUMP_GPS
if(dumpCount==0){
if (fp != NULL) {
fprintf(fp, "%f %f %f %f %f\n",gpsvar.latitude,gpsvar.longitude, gpsvar.course, gpsvar.speed, gpsvar.time);
}
}
dumpCount = dumpCount+1%DUMPGPSDELAY;
#endif //DUMP_GPS
#ifdef DEBUG_TARGET
debugTarget();
#endif //DEBUG_TARGET
#ifdef DEBUG_GUIDE
debugGuide();
#endif //DEBUG_GUIDE
#ifdef DEBUG_GPS
debugGPS();
#endif //DEBUG_GPS
#ifdef DEBUG_LIDAR
debugSICK();
#endif //DEBUG_LIDAR
#ifdef DEBUG_BUFFER
debugCombinedBufferAngles();
#endif //DEBUG_BUFFE
#ifdef DEBUG_VISUALIZER
robotX = currentXY.x;
robotY = currentXY.y;
robotTheta = robotTheta;//redundant I know....
targetX = targetXY.x;
targetY = targetXY.y;
// should probably pass the above to the function...
paintPathPlanner(robotX,robotY,robotTheta);
showPlot();
#endif //VISUALIZER
#ifdef USE_MAP
if(mapCount==0){
// mapRobot(currentXY.x,currentXY.y,robotTheta);
if(clearMapCount==0) clearMapSection(currentXY.x,currentXY.y,robotTheta);
else clearMapCount = (clearMapCount+1)%CLEARMAPDELAY;
mapVSICK(currentXY.x,currentXY.y,robotTheta);
// mapVSICK(0,0,0);
#ifdef USE_LIDAR
mapSICK(currentXY.x,currentXY.y,robotTheta);
#endif
showMap();
// printf("MAPPING\n");
}
mapCount= (mapCount+1)%MAPDELAY;
#endif //USE_MAP
sendSpeed(lSpeed,rSpeed);
Sleep(5);
}
#ifdef DUMP_GPS
fclose(fp);
#endif
return 0;
}
//============================================================================
void AAM_Basic::EstPoseGradientMatrix(CvMat* GPose,
const CvMat* CParams,
const std::vector<AAM_Shape>& AllShapes,
const std::vector<IplImage*>& AllImages,
const CvMat* vPoseDisps)
{
int nExperiment = 0;
int ntotalExp = AllShapes.size()*vPoseDisps->rows/2*vPoseDisps->cols;
int npixels = __cam.__texture.nPixels();
int npointsby2 = __cam.__shape.nPoints()*2;
int smodes = __cam.__shape.nModes();
CvMat c; //appearance parameters
CvMat* q = cvCreateMat(1, 4, CV_64FC1); //pose parameters
CvMat* q1 = cvCreateMat(1, 4, CV_64FC1);
CvMat* q2 = cvCreateMat(1, 4, CV_64FC1);
CvMat* p = cvCreateMat(1, smodes, CV_64FC1);//shape parameters
CvMat* s1 = cvCreateMat(1, npointsby2, CV_64FC1);//shape vector
CvMat* s2 = cvCreateMat(1, npointsby2, CV_64FC1);
CvMat* t1 = cvCreateMat(1, npixels, CV_64FC1);//texture vector
CvMat* t2 = cvCreateMat(1, npixels, CV_64FC1);
CvMat* delta_g1 = cvCreateMat(1, npixels, CV_64FC1);
CvMat* delta_g2 = cvCreateMat(1, npixels, CV_64FC1);
CvMat* cDiff = cvCreateMat(1, npixels, CV_64FC1);
std::vector<double> normFactors; normFactors.resize(4);
CvMat* AbsPoseDisps = cvCreateMat(vPoseDisps->rows, vPoseDisps->cols, CV_64FC1);
// for each training example in the training set
for(int i = 0; i < AllShapes.size(); i++)
{
cvGetRow(CParams, &c, i);
//calculate pose parameters
AllShapes[i].Point2Mat(s1);
__cam.__shape.CalcParams(s1, p, q);
cvCopy(vPoseDisps, AbsPoseDisps);
int w = AllShapes[i].GetWidth();
int h = AllShapes[i].GetHeight();
int W = AllImages[i]->width;
int H = AllImages[i]->height;
for(int j = 0; j < vPoseDisps->rows; j+=2)
{
//translate relative translation to abs translation about x & y
CV_MAT_ELEM(*AbsPoseDisps, double, j, 2) *= w;
CV_MAT_ELEM(*AbsPoseDisps, double, j, 3) *= h;
CV_MAT_ELEM(*AbsPoseDisps, double, j+1, 2) *= w;
CV_MAT_ELEM(*AbsPoseDisps, double, j+1, 3) *= h;
for(int k = 0; k < vPoseDisps->cols; k++)
{
printf("Performing (%d/%d)\r", nExperiment++, ntotalExp);
//shift current pose parameters
cvCopy(q, q1);
cvCopy(q, q2);
if(k == 0 || k == 1){
double scale, theta, dscale1, dtheta1, dscale2, dtheta2;
scale = MAG(cvmGet(q,0,0)+1,cvmGet(q,0,1));
theta = THETA(cvmGet(q,0,0)+1,cvmGet(q,0,1));
dscale1 = MAG(cvmGet(AbsPoseDisps,j,0)+1,cvmGet(AbsPoseDisps,j,1));
dtheta1 = THETA(cvmGet(AbsPoseDisps,j,0)+1,cvmGet(AbsPoseDisps,j,1));
dscale2 = MAG(cvmGet(AbsPoseDisps,j+1,0)+1,cvmGet(AbsPoseDisps,j+1,1));
dtheta2 = THETA(cvmGet(AbsPoseDisps,j+1,0)+1,cvmGet(AbsPoseDisps,j+1,1));
dscale1 = dscale1*scale; dtheta1 += theta;
dscale2 = dscale2*scale; dtheta2 += theta;
if(k == 0){
cvmSet(q1,0,0,dscale1*cos(theta)-1);
cvmSet(q1,0,1,dscale1*sin(theta));
cvmSet(q2,0,0,dscale2*cos(theta)-1);
cvmSet(q2,0,1,dscale2*sin(theta));
}
else{
cvmSet(q1,0,0,scale*cos(dtheta1)-1);
cvmSet(q1,0,1,scale*sin(dtheta1));
cvmSet(q2,0,0,scale*cos(dtheta2)-1);
cvmSet(q2,0,1,scale*sin(dtheta2));
}
}
else
{
cvmSet(q1,0,k,cvmGet(q,0,k)+cvmGet(AbsPoseDisps,j,k));
cvmSet(q2,0,k,cvmGet(q,0,k)+cvmGet(AbsPoseDisps,j+1,k));
}
//generate model texture
__cam.CalcTexture(t1, &c);
__cam.CalcTexture(t2, &c);
// {
// char filename[100];
// sprintf(filename, "a/%d.jpg", nExperiment);
// __cam.__paw.SaveWarpImageFromVector(filename, t1);
// }
//generate model shape
//__cam.__shape.CalcShape(p, q1, s1);
//__cam.__shape.CalcShape(p, q2, s2);
__cam.CalcLocalShape(s1, &c); __cam.__shape.CalcGlobalShape(q1,s1);
__cam.CalcLocalShape(s2, &c); __cam.__shape.CalcGlobalShape(q2,s2);
//sample the shape to get warped texture
if(!AAM_Basic::IsShapeWithinImage(s1, W, H)) cvZero(delta_g1);
else __cam.__paw.FasterGetWarpTextureFromMatShape(s1, AllImages[i], delta_g1, true);
__cam.__texture.AlignTextureToRef(__cam.__MeanG, delta_g1);
if(!AAM_Basic::IsShapeWithinImage(s2, W, H)) cvZero(delta_g2);
else __cam.__paw.FasterGetWarpTextureFromMatShape(s2, AllImages[i], delta_g2, true);
__cam.__texture.AlignTextureToRef(__cam.__MeanG, delta_g2);
// {
// char filename[100];
// sprintf(filename, "a/%d-.jpg", nExperiment);
// __cam.__paw.SaveWarpImageFromVector(filename, delta_g1);
// sprintf(filename, "a/%d+.jpg", nExperiment);
// __cam.__paw.SaveWarpImageFromVector(filename, delta_g2);
// }
//calculate pixel difference(g_s - g_m)
cvSub(delta_g1, t1, delta_g1);
cvSub(delta_g2, t2, delta_g2);
//form central displacement
cvSub(delta_g2, delta_g1, cDiff);
cvConvertScale(cDiff, cDiff, 1.0/(cvmGet(AbsPoseDisps,j+1,k)-cvmGet(AbsPoseDisps,j,k)));
//accumulate into k-th row
CvMat Gk; cvGetRow(GPose, &Gk, k);
cvAdd(&Gk, cDiff, &Gk);
//increment normalisation factor
normFactors[k] = normFactors[k]+1;
}
}
}
//normalize
for(int j = 0; j < vPoseDisps->cols; j++)
{
CvMat Gj; cvGetRow(GPose, &Gj, j);
cvConvertScale(&Gj, &Gj, 1.0/normFactors[j]);
}
cvReleaseMat(&s1); cvReleaseMat(&s2);
cvReleaseMat(&t1); cvReleaseMat(&t2);
cvReleaseMat(&delta_g1); cvReleaseMat(&delta_g2);
cvReleaseMat(&cDiff);
cvReleaseMat(&AbsPoseDisps);
}
static void
show_encounter(RUBBLE_PILE *rp,int n,BOOLEAN sim_units)
{
VECTOR rel_pos,rel_vel,ang_mom;
double m,sr,r,v,eb,a,e,q,Q,vq,vQ;
assert(n == 2);
m = rp[0].mass + rp[1].mass;
sr = rp[0].radius + rp[1].radius;
SUB_VEC(rp[1].pos,rp[0].pos,rel_pos);
SUB_VEC(rp[1].vel,rp[0].vel,rel_vel);
CROSS(rel_pos,rel_vel,ang_mom);
r = MAG(rel_pos);
v = MAG(rel_vel);
if (r == 0) eb = 0;
else eb = 0.5*SQ(v) - m/r;
if (eb == 0) {
a = 0;
e = 1;
}
else {
a = -0.5*m/eb;
e = sqrt(1 - MAG_SQ(ang_mom)/(a*m));
}
q = (1 - e)*a;
Q = (1 + e)*a;
(void) printf("2-body encounter parameters:\n");
(void) printf("Binding energy per unit reduced mass = %g sim units\n",eb);
(void) printf("Semi-major axis a = ");
if (sim_units) (void) printf("%g AU",a);
else (void) printf("%g km",a*0.001*L_SCALE);
(void) printf("\n");
(void) printf("Eccentricity e = %g\n",e);
(void) printf("Close-approach distance q = ");
if (sim_units) (void) printf("%g AU",q);
else (void) printf("%g km",q*0.001*L_SCALE);
if (q <= sr) (void) printf(" (impact possible)");
(void) printf("\n");
(void) printf("Initial relative speed v = ");
if (sim_units) (void) printf("%g x 30 km/s",v);
else (void) printf("%g m/s",v*V_SCALE);
(void) printf("\n");
(void) printf("Relative speed at ");
if (eb < 0) {
vQ = sqrt(2*(eb + m/Q));
(void) printf("apoapse vQ = ");
}
else {
vQ = sqrt(2*eb);
(void) printf("infinity v_inf = ");
}
if (sim_units) (void) printf("%g x 30 km/s",vQ);
else (void) printf("%g m/s",vQ*V_SCALE);
(void) printf("\n");
(void) printf("Relative speed at ");
if (q <= sr) {
if (r > sr) q = sr;
(void) printf("impact (approx)");
}
else (void) printf("periapse");
(void) printf(" vq = ");
if (q < sr) vq = v;
else vq = sqrt(2*(eb + m/q));
if (sim_units) (void) printf("%g x 30 km/s",vq);
else (void) printf("%g m/s",vq*V_SCALE);
(void) printf("\n");
(void) printf("Relative orb. ang. mom. per unit reduced mass h = "
"(%g,%g,%g) sim units\n",ang_mom[X],ang_mom[Y],ang_mom[Z]);
}
static int
process(char *filename,RUBBLE_PILE *rp,BOOLEAN sim_units,double *time)
{
enum {next,mass,radius,density,pos,vel,orient,spin,color,agg_id,par_id};
SSIO ssio;
SSHEAD h;
int i,choice;
assert(rp != NULL);
*time = 0.0;
if (ssioOpen(filename,&ssio,SSIO_READ)) {
(void) fprintf(stderr,"Unable to open \"%s\"\n",filename);
return 1;
}
if (ssioHead(&ssio,&h) || h.n_data < 0) {
(void) fprintf(stderr,"Corrupt header\n");
(void) ssioClose(&ssio);
return 1;
}
if (h.n_data == 0) {
(void) fprintf(stderr,"No data found!");
(void) ssioClose(&ssio);
return 1;
}
switch(h.iMagicNumber) {
case SSIO_MAGIC_STANDARD:
break;
case SSIO_MAGIC_REDUCED:
(void) fprintf(stderr,"Reduced ss format not supported.\n");
ssioClose(&ssio);
return 1;
default:
(void) fprintf(stderr,"Unrecognized ss file magic number (%i).\n",h.iMagicNumber);
ssioClose(&ssio);
return 1;
}
rp->n_particles = h.n_data;
*time = h.time;
(void) printf("Number of particles = %i (time %g)\n",rp->n_particles,*time);
rpuMalloc(rp);
for (i=0;i<rp->n_particles;i++)
if (ssioData(&ssio,&rp->data[i])) {
(void) fprintf(stderr,"Corrupt data\n");
(void) ssioClose(&ssio);
return 1;
}
(void) ssioClose(&ssio);
while (/*CONSTCOND*/1) {
rpuAnalyze(rp);
(void) printf("%i. Total mass = ",mass);
if (sim_units) (void) printf("%g M_sun",rp->mass);
else (void) printf("%g kg",rp->mass*M_SCALE);
(void) printf("\n");
(void) printf("%i. Bulk radius = ",radius);
if (sim_units) (void) printf("%g AU",rp->radius);
else (void) printf("%g km",rp->radius*0.001*L_SCALE);
(void) printf("\n");
(void) printf(" [Bulk semi-axes: ");
if (sim_units) (void) printf("%g %g %g AU",
rp->axis_len[rp->axis_ord[X]],
rp->axis_len[rp->axis_ord[Y]],
rp->axis_len[rp->axis_ord[Z]]);
else (void) printf("%g %g %g km",
rp->axis_len[rp->axis_ord[X]]*0.001*L_SCALE,
rp->axis_len[rp->axis_ord[Y]]*0.001*L_SCALE,
rp->axis_len[rp->axis_ord[Z]]*0.001*L_SCALE);
(void) printf("]\n");
(void) printf("%i. Bulk density = ",density);
if (sim_units) (void) printf("%g M_sun/AU^3",rp->density);
else (void) printf("%g g/cc",rp->density*0.001*D_SCALE);
(void) printf("\n");
(void) printf("%i. Centre-of-mass position = ",pos);
if (sim_units) (void) printf("%g %g %g AU",rp->pos[X],rp->pos[Y],
rp->pos[Z]);
else (void) printf("%.2f %.2f %.2f km",rp->pos[X]*0.001*L_SCALE,
rp->pos[Y]*0.001*L_SCALE,rp->pos[Z]*0.001*L_SCALE);
(void) printf("\n");
(void) printf("%i. Centre-of-mass velocity = ",vel);
if (sim_units) (void) printf("%g %g %g x 30 km/s",rp->vel[X],rp->vel[Y],
rp->vel[Z]);
else (void) printf("%.2f %.2f %.2f m/s",rp->vel[X]*V_SCALE,
rp->vel[Y]*V_SCALE,rp->vel[Z]*V_SCALE);
(void) printf("\n");
(void) printf("%i. Orientation: a1 = %6.3f %6.3f %6.3f\n",orient,
rp->axes[rp->axis_ord[X]][X],
rp->axes[rp->axis_ord[X]][Y],
rp->axes[rp->axis_ord[X]][Z]);
(void) printf(" a2 = %6.3f %6.3f %6.3f\n",
rp->axes[rp->axis_ord[Y]][X],
rp->axes[rp->axis_ord[Y]][Y],
rp->axes[rp->axis_ord[Y]][Z]);
(void) printf(" a3 = %6.3f %6.3f %6.3f\n",
rp->axes[rp->axis_ord[Z]][X],
rp->axes[rp->axis_ord[Z]][Y],
rp->axes[rp->axis_ord[Z]][Z]);
(void) printf("%i. Spin = ",spin);
if (sim_units) (void) printf("%g %g %g x 2pi rad/yr",
rp->spin[X],rp->spin[Y],rp->spin[Z]);
else {
double scale = 3600/(TWO_PI*T_SCALE),w;
(void) printf("%.2f %.2f %.2f 1/h (",rp->spin[X]*scale,
rp->spin[Y]*scale,rp->spin[Z]*scale);
w = MAG(rp->spin);
if (w) (void) printf("period %g h",1/(w*scale));
else (void) printf("no spin");
(void) printf(")");
}
(void) printf("\n");
(void) printf(" [Ang mom = ");
if (sim_units) (void) printf("%g %g %g (sys units)",rp->ang_mom[X],
rp->ang_mom[Y],rp->ang_mom[Z]);
else {
double scale = M_SCALE*SQ(L_SCALE)/T_SCALE;
(void) printf("%.5e %.5e %.5e N m/s",rp->ang_mom[X]*scale,
rp->ang_mom[Y]*scale,rp->ang_mom[Z]*scale);
}
(void) printf("]\n");
(void) printf(" [Effective spin = ");
if (sim_units) (void) printf("%g x 2pi rad/yr",rp->eff_spin);
else {
double scale = 3600/(TWO_PI*T_SCALE);
(void) printf("%.2f 1/h (",rp->eff_spin*scale);
if (rp->eff_spin) printf("period %g h",1/(rp->eff_spin*scale));
else (void) printf("no spin");
(void) printf(")");
}
(void) printf("]\n");
(void) printf(" [Rotation index = %.2f (",rp->rot_idx);
if (rp->eff_spin == 0.0) (void) printf("undefined");
else if (rp->rot_idx == 1.0) (void) printf("unif rot about max moment");
else if (rp->rot_idx < 1.0 && rp->rot_idx > 0.0) (void) printf("SAM");
else if (rp->rot_idx == 0.0) (void) printf("unif rot about mid moment");
else if (rp->rot_idx < 0.0 && rp->rot_idx > -1.0) (void) printf("LAM");
else if (rp->rot_idx == -1.0) (void) printf("unif rot about min moment");
else assert(0);
(void) printf(")]\n");
(void) printf("%i. Color = %i (%s)\n",color,(int) rp->color,
color_str(rp->color));
(void) printf("%i. Aggregate ID = ",agg_id);
if (rp->agg_id < 0)
(void) printf("N/A");
else
(void) printf("%i",(int) rp->agg_id);
(void) printf("\n");
(void) printf("%i. Particle ID\n",par_id);
do {
(void) printf("Enter number to change (or 0 to continue): ");
(void) scanf("%i",&choice);
(void) getchar();
} while (choice < next || choice > par_id);
if (choice == next) return 0;
switch(choice) {
case mass:
{
double f;
BOOLEAN const_den = get_yn("Keep bulk density constant","n"),proceed=TRUE;
do {
(void) printf("Enter mass scaling factor (-ve ==> abs val): ");
(void) scanf("%lf",&f);
if (f == 0.0) {
getchar();
proceed = get_yn("WARNING: This will set all particle masses to zero...continue","n");
}
} while (!proceed);
if (f < 0) {
if (!sim_units) f /= M_SCALE;
f = -f/rp->mass;
}
rpuScaleMass(rp,f);
if (const_den) rpuScaleRadius(rp,pow(f,1.0/3),FALSE);
break;
}
case radius:
{
double f;
BOOLEAN just_particles = get_yn("Just scale particles","n"),const_den = FALSE;
if (!just_particles)
const_den = get_yn("Keep bulk density constant","n");
do {
(void) printf("Enter radius scaling factor "
"(-ve ==> abs val): ");
(void) scanf("%lf",&f);
} while (f == 0);
getchar();
if (f < 0) {
if (!sim_units) f *= 1000/L_SCALE;
if (!just_particles)
f = -f/rp->radius;
}
rpuScaleRadius(rp,f,just_particles);
if (just_particles)
rpuCalcRadius(rp); /* because outer edge may have changed */
if (const_den) rpuScaleMass(rp,CUBE(f));
break;
}
case density:
{
double f;
BOOLEAN const_radius = get_yn("Keep radius constant","y");
do {
(void) printf("Enter density scaling factor (-ve ==> abs val): ");
(void) scanf("%lf",&f);
} while (f == 0);
getchar();
if (f < 0) {
if (!sim_units) f *= 1000/D_SCALE;
f = -f/rp->density;
}
if (const_radius) rpuScaleMass(rp,f);
else rpuScaleRadius(rp,pow(f,-1.0/3),FALSE);
break;
}
case pos:
{
BOOLEAN absolute = get_yn("Specify absolute position","y");
if (absolute) {
SCALE_VEC(rp->pos,-1.0);
rpuApplyPos(rp); /* reset COM to (0,0,0) first */
(void) printf("Enter new position [x y z in ");
}
else
(void) printf("Enter position offset [x y z in ");
if (sim_units) (void) printf("AU");
else (void) printf("km");
(void) printf("]: ");
(void) scanf("%lf%lf%lf",&rp->pos[X],&rp->pos[Y],&rp->pos[Z]);
(void) getchar();
if (!sim_units) NORM_VEC(rp->pos,0.001*L_SCALE);
rpuApplyPos(rp);
break;
}
case vel:
{
BOOLEAN absolute = get_yn("Specify absolute velocity","y");
if (absolute) {
SCALE_VEC(rp->vel,-1.0);
rpuApplyVel(rp);
(void) printf("Enter new velocity [vx vy vz in ");
}
else
(void) printf("Enter velocity offset [vx vy vz in ");
if (sim_units) (void) printf("units of 30 km/s");
else (void) printf("m/s");
(void) printf("]: ");
(void) scanf("%lf%lf%lf",&rp->vel[X],&rp->vel[Y],&rp->vel[Z]);
(void) getchar();
if (!sim_units) NORM_VEC(rp->vel,V_SCALE);
rpuApplyVel(rp);
break;
}
case orient:
{
enum {x=1,y,z};
MATRIX rot;
double angle;
BOOLEAN align,body,rndm;
int choice;
align = get_yn("Align body axes with coordinate axes","n");
if (align) {
MATRIX m;
COPY_VEC(rp->axes[MAJOR(rp)],m[X]);
COPY_VEC(rp->axes[INTER(rp)],m[Y]);
COPY_VEC(rp->axes[MINOR(rp)],m[Z]);
rpuRotate(rp,m,FALSE);
break;
}
body = get_yn("Use body axes","n");
(void) printf("%i. Rotate about %s axis\n",x,body?"major":"x");
(void) printf("%i. Rotate about %s axis\n",y,body?"intermediate":"y");
(void) printf("%i. Rotate about %s axis\n",z,body?"minor":"z");
do {
(void) printf("Enter choice: ");
(void) scanf("%i",&choice);
} while (choice < x || choice > z);
--choice; /* to conform with X,Y,Z macros */
(void) getchar();
rndm = get_yn("Use random angle","n");
if (rndm) angle = TWO_PI*rand()/RAND_MAX;
else {
(void) printf("Enter rotation angle in ");
if (sim_units) (void) printf("radians");
else (void) printf("degrees");
(void) printf(" (-ve=clockwise): ");
(void) scanf("%lf",&angle);
(void) getchar();
if (!sim_units) angle *= DEG_TO_RAD;
}
UNIT_MAT(rot);
if (body) choice = rp->axis_ord[choice];
switch (choice) {
case X:
rot[Y][Y] = cos(angle); rot[Y][Z] = -sin(angle);
rot[Z][Y] = -rot[Y][Z]; rot[Z][Z] = rot[Y][Y];
break;
case Y:
rot[X][X] = cos(angle); rot[X][Z] = sin(angle);
rot[Z][X] = -rot[X][Z]; rot[Z][Z] = rot[X][X];
break;
case Z:
rot[X][X] = cos(angle); rot[X][Y] = -sin(angle);
rot[Y][X] = -rot[X][Y]; rot[Y][Y] = rot[X][X];
break;
default:
assert(0);
}
rpuRotate(rp,rot,body);
break;
}
case spin:
{
VECTOR old_spin,d;
double w_max = 2*PI/sqrt(3*PI/rp->density);
BOOLEAN incr_only,ang_mom,body;
COPY_VEC(rp->spin,old_spin); /* needed for spin increment */
/*DEBUG following only removes net spin---it does not
ensure that every particle has wxr_i = 0 and w_i = 0*/
SCALE_VEC(rp->spin,-1.0);
rpuAddSpin(rp,FALSE); /* remove current spin */
(void) printf("Classical breakup limit for measured density = ");
if (sim_units) (void) printf("%g x 2pi rad/yr",w_max);
else (void) printf("%g 1/h (P_min = %g h)",
3600*w_max/(TWO_PI*T_SCALE),
TWO_PI*T_SCALE/(3600*w_max));
(void) printf("\n");
incr_only = get_yn("Specify increment only, instead of absolute value","n");
if (incr_only)
ang_mom = get_yn("Specify angular momentum increment","n");
else
ang_mom = get_yn("Specify angular momentum","n");
if (ang_mom) {
if (incr_only)
(void) printf("Enter angular momentum increment [dlx dly dlz in ");
else
(void) printf("Enter new angular momentum [lx ly lz in ");
if (sim_units) (void) printf("sys units");
else (void) printf("N m/s");
(void) printf("]: ");
(void) scanf("%lf%lf%lf",&d[X],&d[Y],&d[Z]);
(void) getchar();
if (!sim_units)
SCALE_VEC(d,T_SCALE/(SQ(L_SCALE)*M_SCALE));
if (incr_only) {
ADD_VEC(rp->ang_mom,d,rp->ang_mom);
}
else {
COPY_VEC(d,rp->ang_mom);
}
rpuAddAngMom(rp);
if (MAG(rp->spin) > w_max)
(void) printf("WARNING: exceeds classical breakup limit\n");
break;
}
body = get_yn("Use body axes","n");
if (incr_only)
(void) printf("Enter spin increment [dwx dwy dwz in ");
else
(void) printf("Enter new spin [wx wy wz in ");
if (sim_units) (void) printf("units of 2pi rad/yr");
else (void) printf("1/h");
(void) printf("]: ");
(void) scanf("%lf%lf%lf",&d[X],&d[Y],&d[Z]);
(void) getchar();
if (!sim_units) SCALE_VEC(d,TWO_PI*T_SCALE/3600);
if (incr_only) {
ADD_VEC(old_spin,d,rp->spin);
}
else {
COPY_VEC(d,rp->spin);
}
if (MAG(rp->spin) > w_max)
(void) printf("WARNING: exceeds classical breakup limit\n");
rpuAddSpin(rp,body);
break;
}
case color:
{
BOOLEAN invalid_color;
int c;
(void) printf("Color scheme:\n");
for (i=BLACK;i<FIRST_GRAY;i++)
(void) printf("%2i. %s\n",i,color_str(i));
do {
invalid_color = FALSE;
(void) printf("Enter new color: ");
(void) scanf("%i",&c);
(void) getchar();
if (c >= NUM_COLORS) { /* allow negative colors */
(void) printf("Invalid color\n");
invalid_color = TRUE;
continue;
}
if (c == BLACK || c == FIRST_GRAY)
(void) printf("WARNING: Particles may be invisible!\n");
} while (invalid_color);
rp->color = c;
rpuApplyColor(rp);
break;
}
case agg_id:
{
do {
(void) printf("Enter new aggregate ID (or -1 to reset): ");
(void) scanf("%i",&rp->agg_id);
(void) getchar();
if (rp->agg_id < -1)
(void) printf("Invalid ID\n");
else
rpuApplyAggID(rp);
} while(rp->agg_id < -1);
break;
}
case par_id:
{
SSDATA *p,*pmax;
VECTOR r;
double lng,lat,dmax,d;
int c;
(void) printf("Enter angular coordinates [lng lat in deg]: ");
(void) scanf("%lf%lf",&lng,&lat);
(void) getchar();
lng *= DEG_TO_RAD;
lat *= DEG_TO_RAD;
SET_VEC(r,cos(lng)*cos(lat),sin(lng)*cos(lat),sin(lat));
dmax = 0.0;
pmax = NULL;
for (i=0;i<rp->n_particles;i++) {
p = &rp->data[i];
/* projected distance along vector minus distance perpendicular
to vector favors particles closest to vector */
d = DOT(p->pos,r) - sqrt(MAG_SQ(p->pos) - SQ(DOT(p->pos,r)));
if (d > dmax) {
dmax = d;
pmax = p;
}
}
if (!pmax) {
(void) printf("No particle found.\n");
continue;
}
(void) printf("Found particle (original index %i, color %i [%s])\n",
pmax->org_idx,pmax->color,color_str(pmax->color));
while (/*CONSTCOND*/1) {
(void) printf("Enter new color: ");
(void) scanf("%i",&c);
(void) getchar();
if (c >= NUM_COLORS) { /* allow negative colors */
(void) printf("Invalid color\n");
goto invalid;
}
if (c == BLACK || c == RED || c == YELLOW || c == MAGENTA ||
c == CYAN || c == KHAKI || c == FIRST_GRAY) {
(void) printf("Color %i is reserved\n",c);
goto invalid;
}
break;
invalid:
(void) printf("Color scheme:\n");
for (i=BLACK;i<FIRST_GRAY;i++)
(void) printf("%2i. %s\n",i,color_str(i));
}; /* while */
pmax->color = c;
break;
}
default:
assert(0);
}
}
}
static void
process(SSDATA *d,int n,double *t)
{
PROPERTIES p;
int choice;
enum {Next,Time,Mass,Bounds,ComPos,ComVel,AngMom,VelDsp,Color,Units,
Offsets,Masses,Radii,End};
while (/*CONSTCOND*/1) {
ss_analyze(d,n,&p);
(void) printf("%2i. Time = %g\n",Time,*t);
(void) printf("%2i. Total mass = %g\n",Mass,p.total_mass);
(void) printf("%2i. Bounds: x=[%g,%g]\n"
" y=[%g,%g]\n"
" z=[%g,%g]\n",Bounds,
p.bnd_min[X],p.bnd_max[X],
p.bnd_min[Y],p.bnd_max[Y],
p.bnd_min[Z],p.bnd_max[Z]);
(void) printf("%2i. Centre-of-mass position = %g %g %g\n",ComPos,
p.com_pos[X],p.com_pos[Y],p.com_pos[Z]);
(void) printf("%2i. Centre-of-mass velocity = %g %g %g\n",ComVel,
p.com_vel[X],p.com_vel[Y],p.com_vel[Z]);
(void) printf("%2i. Specific angular momentum = %g %g %g\n",AngMom,
p.ang_mom[X],p.ang_mom[Y],p.ang_mom[Z]);
(void) printf("%2i. Velocity dispersion = %g %g %g\n",VelDsp,
p.vel_dsp[X],p.vel_dsp[Y],p.vel_dsp[Z]);
(void) printf("%2i. Dominant color = %i (%s)\n",Color,p.color,
color_str(p.color));
(void) printf("%2i. Units\n",Units);
(void) printf("%2i. Offsets\n",Offsets);
(void) printf("%2i. Particle masses\n",Masses);
(void) printf("%2i. Particle radii\n",Radii);
do {
(void) printf("Enter number to change (or 0 to continue): ");
(void) scanf("%i",&choice);
} while (choice < Next || choice >= End);
(void) getchar();
if (choice == Next) return;
switch(choice) {
case Time:
do {
(void) printf("Enter new time: ");
(void) scanf("%lf",t);
(void) getchar();
} while (*t < 0);
break;
case Mass:
{
double f;
do get_scaling(&f,NegativeOK);
while (f == 0);
if (f < 0) f = -f/p.total_mass;
scale_mass(d,n,f);
break;
}
case Bounds:
{
double f,min,max;
int i,choice;
do {
do {
(void) printf("%i. Change x bounds (now [%g,%g])\n",X + 1,
p.bnd_min[X],p.bnd_max[X]);
(void) printf("%i. Change y bounds (now [%g,%g])\n",Y + 1,
p.bnd_min[Y],p.bnd_max[Y]);
(void) printf("%i. Change z bounds (now [%g,%g])\n",Z + 1,
p.bnd_min[Z],p.bnd_max[Z]);
(void) printf("Your choice (or 0 when done): ");
(void) scanf("%i",&choice);
(void) getchar();
} while (choice < 0 || choice > N_DIM);
if (choice == 0) break;
--choice; /* put back in range [X,Z] */
if (p.bnd_min[choice] == p.bnd_max[choice]) {
(void) printf("Chosen dimension is degenerate\n");
continue;
}
do {
(void) printf("Enter new bounds (min max): ");
(void) scanf("%lf%lf",&min,&max);
(void) getchar();
} while (min > max);
if (min == max &&
get_yn("Zero velocities for this component","y"))
for (i=0;i<n;i++)
d[i].vel[choice] = 0;
f = (max - min)/(p.bnd_max[choice] - p.bnd_min[choice]);
for (i=0;i<n;i++)
d[i].pos[choice] =
(d[i].pos[choice] - p.bnd_min[choice])*f + min;
p.bnd_min[choice] = min;
p.bnd_max[choice] = max;
} while (/*CONSTCOND*/1);
break;
}
case ComPos:
{
VECTOR v;
if (MAG(p.com_pos) && get_yn("Scale the magnitude","y")) {
double f;
get_scaling(&f,NegativeOK);
COPY_VEC(p.com_pos,v);
if (f < 0) f = -f/MAG(v);
SCALE_VEC(v,f);
}
else get_components(v);
adj_com_pos(d,n,&p,v);
break;
}
case ComVel:
{
VECTOR v;
if (MAG(p.com_vel) && get_yn("Scale the magnitude","y")) {
double f;
get_scaling(&f,NegativeOK);
COPY_VEC(p.com_vel,v);
if (f < 0) f = -f/MAG(v);
SCALE_VEC(v,f);
}
else get_components(v);
adj_com_vel(d,n,&p,v);
break;
}
case AngMom:
{
VECTOR v;
(void) printf("NOTE: specific angular momentum is measured with\n"
"respect to fixed space frame centred at (0,0,0)\n"
"and does not take particle spins into account\n");
if (MAG(p.ang_mom) && get_yn("Scale the magnitude","y")) {
double f;
get_scaling(&f,NegativeOK);
COPY_VEC(p.ang_mom,v);
if (f < 0) f = -f/MAG(p.ang_mom);
SCALE_VEC(v,f);
}
else if (get_yn("Scale the components","y")) {
VECTOR u;
int k;
get_component_scaling(u);
for (k=0;k<N_DIM;k++)
v[k] = u[k]*p.ang_mom[k];
}
else get_components(v);
adj_ang_mom(d,n,&p,v);
break;
}
case VelDsp:
{
VECTOR v;
(void) printf("NOTE: velocity dispersion is context dependent,\n"
"for now relative ONLY to center-of-mass velocity,\n"
"i.e. without considering bulk rotation or shear\n");
if (!MAG(p.vel_dsp)) {
(void) printf("Zero velocity dispersion -- cannot adjust\n");
break;
}
if (get_yn("Scale the magnitude","y")) {
double f;
get_scaling(&f,NegativeOK);
if (f < 0) f = -f/MAG(p.vel_dsp);
SET_VEC(v,f,f,f);
}
else get_component_scaling(v);
scale_vel_dsp(d,n,&p,v);
break;
}
case Color:
{
int c;
(void) printf("Color scheme:\n");
for (c=BLACK;c<FIRST_GRAY;c++)
(void) printf("%2i. %s\n",c,color_str(c));
(void) printf("[values from %i to %i are levels of gray]\n",
FIRST_GRAY,LAST_GRAY);
do {
(void) printf("Enter new color: ");
(void) scanf("%i",&c);
(void) getchar();
} while (c < 0 || c >= NUM_COLORS);
change_color(d,n,c);
break;
}
case Units:
{
enum {N,M,L,T,V,E};
double f;
int i,choice;
(void) printf("NOTE: It is up to you to ensure dimensions are\n"
"internally consistent. pkdgrav assumes G == 1.\n");
do {
do {
(void) printf("%i. Mass (particle masses)\n",M);
(void) printf("%i. Length (particle radii, pos'ns)\n",L);
(void) printf("%i. Time (time,particle spins)\n",T);
(void) printf("%i. Velocity (particle velocities)\n",V);
(void) printf("Select dimension to scale "
"(or 0 when done): ");
(void) scanf("%i",&choice);
(void) getchar();
} while (choice < N || choice >= E);
if (choice == N) break;
switch (choice) {
case M:
(void) printf("M_Sun = 1.9891e30 kg\n"
"M_Earth = 5.9742e24 kg\n"
"M_Jupiter = 1.8992e27 kg\n"
"M_Saturn = 5.6864e26 kg\n");
get_scaling(&f,PositiveOnly);
for (i=0;i<n;i++)
d[i].mass *= f;
break;
case L:
(void) printf("1 AU = 1.49597892e11 m\n"
"R_Earth = 6.37814e6 m\n");
get_scaling(&f,PositiveOnly);
for (i=0;i<n;i++) {
d[i].radius *= f;
SCALE_VEC(d[i].pos,f);
}
break;
case T:
(void) printf("1 yr = 3.15576e7 s\n"
"1 yr / 2 pi = 5.02255e6 s\n");
get_scaling(&f,PositiveOnly);
*t *= f;
for (i=0;i<n;i++)
NORM_VEC(d[i].spin,f);
break;
case V:
(void) printf("V_Earth = 2.97852586e4 m/s\n");
get_scaling(&f,PositiveOnly);
for (i=0;i<n;i++)
SCALE_VEC(d[i].vel,f);
break;
default:
assert(0);
}
} while (/*CONSTCOND*/1);
break;
}
case Offsets:
{
VECTOR v;
int i;
(void) printf("POSITION OFFSET (0 0 0 for none)...\n");
get_components(v);
for (i=0;i<n;i++)
ADD_VEC(d[i].pos,v,d[i].pos);
(void) printf("VELOCITY OFFSET (0 0 0 for none)...\n");
get_components(v);
for (i=0;i<n;i++)
ADD_VEC(d[i].vel,v,d[i].vel);
break;
}
case Masses:
{
double f;
do get_scaling(&f,NegativeOK);
while (f == 0);
scale_masses(d,n,f);
break;
}
case Radii:
{
double f;
do get_scaling(&f,NegativeOK);
while (f == 0);
scale_radii(d,n,f);
break;
}
default:
assert(0);
}
}
}
