#include #include #include "ZRGame.h" #include "ZR_API.h" #include "spheres_types.h" #include "spheres_constants.h" #include "ctrl_attitude.h" #include "ctrl_position.h" #include "find_state_error.h" #include "math_matrix.h" #ifdef ZRSIMULATION extern void _init(void); void *_start = &_init; #endif #undef ZRSIMULATION static int firstrun; //DECL::VAR::firstrun static float endgamegainP; //DECL::VAR::endgamegainP static float endgamegainD; //DECL::VAR::endgamegainD static float approachfactor; //DECL::VAR::approachfactor static int needwaypoint; //DECL::VAR::needwaypoint static int lookahead; //DECL::VAR::lookahead static float gainfactorA; //DECL::VAR::gainfactorA static float distance[210]; //DECL::VAR::distance static int obscured[210]; //DECL::VAR::obscured static int giveup; //DECL::VAR::giveup static int t; //DECL::VAR::t static float gainfactor; //DECL::VAR::gainfactor static int inCapture; //DECL::VAR::inCapture static float capStats[4]; //DECL::VAR::capStats static float totInCapture; //DECL::VAR::totInCapture static float veldot[210]; //DECL::VAR::veldot static int inboundsAP[210]; //DECL::VAR::inboundsAP static int inboundsCP[210]; //DECL::VAR::inboundsCP void ZRUser01() { //BEGIN::PROC::ZRUser // // WEEK 3 SUBMISSION FOR ZRASC TEAM "The Catcher in the Sky" // (The team formerly known as Kuhlschrank!!!) // // Pope John XXIII High School, Sparta High School, and Newton High School // (Sparta area, New Jersey) // // Wow this was a real battle with Yobotics right up the end, I hope we win, those // guys are tough. But if we have to lose at least it will probably be to another // team from the great state of New Jersey. // // This was originally based on the MIT Zero Robotics Autonomous Space Capture Challenge // sample program. // Some portions of code were inspired by ideas contained in protocolocon's week 1 winning submission. // Some portions of code were inspired by ideas contained in yobotics! week 2 winning submission // The basic strategy is to estimate the future position of the capture area plus an offset. // Using a simple heuristic, score a number of future positions looking for one that is // "easiest" and then plot a course to that position. // // Once the sphere gets to the approach point, the offset is slowly reduced until docking occurs. // Gains are adjusted in order to lock on the target without using too much fuel. Faster spinning // pods generally require higher gains to obtain a stable lock on the capture area. // state_vector objState; state_vector refState; state_vector capState; state_vector tmp; state_vector targState; float dbg[7]; float offset; // target dist away from capture area until you're in cone float attgainramp = 1.0f; #define GAMETIME 210 // number of seconds that the game lasts #define DOCKTIME 8 // once we are at the approach point, how long to dock #define APPROACHDIST 0.10f // distance from cap area to approach point #define ADJUSTGAINSTIME 1 // boost gains at t <= this time to lock on pod #define MAXPOSGAIN 4.0f // don't let position D gain go higher than this or things get unstable and uses too much fuel #define CHUNKTIME 90 // max trip planning units per initial time slice //DEBUG(("t=%d\n", t)); ///////////////////////////////////////////////////////////////////////////////// // ROUTE PLANNING ///////////////////////////////////////////////////////////////////////////////// if (firstrun < GAMETIME) { state_vector priorsim, cursim; float wpod; int i, firsttry; int score = 0, best_score = 100000, best_t = -1; //ZRTic(); ACGetObjectState(tmp); ACPredictState(firstrun, tmp, priorsim); if (firstrun == 1) { // very first of the first runs, init some stuff for (i = 0; i < GAMETIME; i++) { inboundsAP[i] = 0; distance[i] = 10000.0f; obscured[i] = -1; inboundsCP[i] = 0; } DEBUG(("FROBJ: x=%3.3f z=%3.3f vx=%3.3f vy=%3.3f vz=%3.3f wx=%3.3f wy=%3.3f wz=%3.3f\n", tmp[0], tmp[1], tmp[2], tmp[3], tmp[4], tmp[5], tmp[10], tmp[11], tmp[12])); // for very fast spinning objects we need a larger approach distance and longer time of approach wpod = mathVecMagnitude(&tmp[RATE_Y], 2); if (wpod > 0.10) { // probably our own scenario approachfactor = 2.3; DEBUG(("Super-fast Pod |w|=%3.3f, AF=%3.3f\n", wpod, approachfactor)); } else if (wpod > 0.08) { approachfactor = 2.2; DEBUG(("Fast Pod |w|=%3.3f, AF=%3.3f\n", wpod, approachfactor)); } else if (wpod > 0.069) { approachfactor = 1.5; DEBUG(("Medium Fast Pod |w|=%3.3f, AF=%3.3f\n", wpod, approachfactor)); } else if (wpod > 0.05) { approachfactor = 1.25; DEBUG(("Medium Pod |w|=%3.3f, AF=%3.3f\n", wpod, approachfactor)); } else if (wpod < 0.04) { approachfactor = 1.0; DEBUG(("Slow Pod |w|=%3.3f, AF=%3.3f\n", wpod, approachfactor)); } } for (firsttry = firstrun ; firstrun < GAMETIME; firstrun++) { float dpod, dcap; //if (ZRTocUs() > 4000) { // running out of time!!! return and calc again next timeslice //DEBUG(("Out of time at firstrun = %d\n", firstrun)); //return; //} if ( firsttry != firstrun && (firstrun % CHUNKTIME) == 0) { DEBUG(("Out of time at firstrun = %d\n", firstrun)); } ACPredictState(1, priorsim, cursim); ACGetCaptureState(cursim, capState); memcpy(&priorsim[0], &cursim[0], sizeof(state_vector)); #define MARGINCP 0.03f // see if the capture point is in bounds if (capState[0]+MARGINCP < BOUND_X && capState[0]-MARGINCP > 0-BOUND_X && capState[1]+MARGINCP-0.6 < BOUND_Y && capState[1]-MARGINCP-0.6 > 0-BOUND_Y && capState[2]+MARGINCP < BOUND_Z && capState[2]-MARGINCP > 0-BOUND_Z) { inboundsCP[firstrun] = 1; } else { inboundsCP[firstrun] = 0; //DEBUG(("CPBND T=%d x=%3.3f y=%3.3f z=%3.3f BY=%3.3f\n", firstrun, capState[0], capState[1], capState[2], BOUND_Y)); } // project out from the capture state to the approach point capState[0] -= 4*APPROACHDIST*approachfactor*(cursim[0]-capState[0]); capState[1] -= 4*APPROACHDIST*approachfactor*(cursim[1]-capState[1]); capState[2] -= 4*APPROACHDIST*approachfactor*(cursim[2]-capState[2]); dpod = mathVecMagnitude(cursim, 3); dcap = mathVecMagnitude(capState, 3); distance[firstrun] = dcap; if (dcap + 0.1 < dpod) { // the cap is closer than the pod obscured[firstrun] = 0; } else { // the pod is in the way obscured[firstrun] = 1; } // If the pod goes out of bounds we're all done since the sim ends if (cursim[0] < BOUND_X && cursim[0] > 0-BOUND_X && cursim[1]-0.6 < BOUND_Y && cursim[1]-0.6 > 0-BOUND_Y && cursim[2] < BOUND_Z && cursim[2] > 0-BOUND_Z) { // do nothing, we're good } else { DEBUG(("OUCH: THE POD GOES OUT OF BOUNDS AT elapsed time=%d\n", firstrun+1)); firstrun = GAMETIME+1; break; // abort all further computations } // it's better to try to dock when the approach point is moving away from the tender // because otherwise you have to reverse your velocity. So, project out from the cap // to find the velocity of the approach point, then take the dot product with the relative // position to get the angle between the vectors. The smaller the better. capState[VEL_X] *= (APPROACHDIST*approachfactor + 0.25)/0.25; capState[VEL_Y] *= (APPROACHDIST*approachfactor + 0.25)/0.25; capState[VEL_Z] *= (APPROACHDIST*approachfactor + 0.25)/0.25; veldot[firstrun] = mathVecInner(&capState[VEL_X], &capState[POS_X], 3); veldot[firstrun] /= mathVecMagnitude(&capState[VEL_X], 3); veldot[firstrun] /= mathVecMagnitude(&capState[POS_X], 3); veldot[firstrun] = acosf(veldot[firstrun])/3.14159265; // see if the approach point is in bounds. We have not moved yet so add -0.6 to y to get // absolute position (error free!) #define MARGIN 0.05f // see if the approach point is in bounds if (capState[0]+MARGIN < BOUND_X && capState[0]-MARGIN > 0-BOUND_X && capState[1]+MARGIN-0.6 < BOUND_Y && capState[1]-MARGIN-0.6 > 0-BOUND_Y && capState[2]+MARGIN < BOUND_Z && capState[2]-MARGIN > 0-BOUND_Z) { inboundsAP[firstrun] = 1; } else { inboundsAP[firstrun] = 0; } } // the following loop prints out all the trip planning data // and should be commented out before submitting if (0) { for (i = 1; i < GAMETIME; i++) { DEBUG(("t=%d inAP=%d inCP=%d dist=%3.2f obsc=%d veldot=%3.2f\n", i, inboundsAP[i], inboundsCP[i], distance[i], obscured[i], veldot[i] )); } } // // we finished all the calcs, find the best time to dock // #define MINDOCKTIME 25 // very hard to get to the pod faster than this #define MAXDOCKTIME 180 // probably not enough time to dock past this for (i = MINDOCKTIME; i < MAXDOCKTIME; i++) { int scan; int min_inbounds_time = approachfactor*DOCKTIME; // time to dock + safety margin if (!inboundsCP[i]) continue; // can't dock if the capture point is not in bounds // there have to be at least mi_inbounds_time consecutive seconds where the // approach point remains in bounds or it's really impossible to dock for (scan = i; scan < i+min_inbounds_time; scan++) { if (!inboundsAP[scan]) break; } if (scan < i+min_inbounds_time) continue; // no it is not in bounds long enough // there is some additional time needed when at least the capture point is in bounds #define MIN_CP_INBOUNDS 25 for (scan = i+min_inbounds_time; scan < i+min_inbounds_time + MIN_CP_INBOUNDS; scan++) { if (!inboundsCP[scan]) break; } if (scan < i+min_inbounds_time + MIN_CP_INBOUNDS) continue; // no it is not in bounds long enough score = distance[i]; if (obscured[i]) { score += 3; // big penalty } score += 2*veldot[i]; // it's easier to dock if it's moving away from us if (i < 90) { // early docks consume a lot of fuel, penalize score += (90.0f-(float)i)/90.0f; } if (i > 170) { // late docks risk not completing in time score += ((float)i-170.0f)/20.0f; } if (score < best_score) { best_score = score; best_t = i; } } t = best_t; DEBUG(("Best approach time = %d\n", t)); if (t == -1) { giveup++; // it's impossible to dock DEBUG(("Trip planning has determined that docking is infeasable. Deactivating tender.\n")); } if (score < 3.0) { needwaypoint = 0; } else { needwaypoint = 1; } if (t < 60) { gainfactor = 4.0f; // early dock requires high gains to get there in time } if (t > 140) { gainfactor = 1.0f; // leisurly docking time requires less gain } } ///////////////////////////////////////////////////////////////////////////////// // END OF ROUTE PLANNING ///////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////// // START OF MAIN NAVIGATION CODE // ///////////////////////////////////////////////////////////////////////////////// // The variable "t" starts at the number of seconds until we plan to be at the // approach point (an offset from the capture area). "t" ends up being our state // variable and its meaning is as follows: // // t > 0 // when t > 0 this means we are going to the approach point which is an // offset from the capture area. t counts down as we make this journey. // We use a very low gain setting to conserve fuel since we're just going // in a straight line to meet the pod at the position determined by trip // planning previously. // // t == 0 // when t == 0 we expect to be at the approach point. Shortly before t == 0 // the gains are boosted up (a lot) so we can start locking on the target // // t < 0 && t > -DOCKTIME // t continues to count down into negative values, and the // offset to the capture area is slowly decreased between t == 0 and t == -DOCKTIME // During this time range, if we are not in the cone then we boost up gains // to try to get a better lock and avoid violating constraints. // // t == -DOCKTIME // when t reaches -DOCKTIME we expect to be in the capture area. If we're not, then // we are in trouble. Again, gains get boosted if we're failing to lock on. /////////////////////////////////////////////////////////////////////////////////////// t--; // pseudo-time variable if (giveup) { //DEBUG(("It's impossible for me to dock so I give up.\n")); return; } // Use very low gains at first to save fuel. // Because the initial journey to the approach point is a straight line // and we're going quite slow, there is really no reason to use any // I (integration) or D (derivative) gain factors at all. It seems to // actually work better with those set to zero. Later during dock we'll // turn up the gains. ZRSetPosGains(gainfactor*0.6*0.0172f, 0.0f, 0.0f); ZRSetAttGains(0.1f*0.0045f, 0.1f*0.00018f, 0.1f*0.0143f); //=== Create State "Measurement" for Controller ===// //Get SPHERES state to set the attitude measurement //(position and other items will be overwritten below) ZRGetMySphState(refState); //Relative state of object in tender frame (with global attitude) ACGetObjectState(tmp); // Compute where the object is expected to be when we're going to be ready to dock if (t > 0) { ACPredictState(t, tmp, objState); } else { // always project a little forward to keep up with faster spins // we played with a lot of strategies on this, but so far lookahead == 1 seems to give // the best results, even for very fast spins ACPredictState(lookahead, tmp, objState); } ACGetCaptureState(objState, capState); // we want to go to an approach point that is offset from the capture area by // APPROACHDIST*approachfactor. // // Once t == 0 we start decreasing the size of this approach point so that // we dock at about t = -DOCKTIME*approachfactor // // The approachfactor can be used to increase the docktime and approach distance, this // is useful for fast spins. It gives the "I" factor of PID control more time to estimate // the error properly. if (t < 0 && t > -DOCKTIME*approachfactor) { //offset = (16.0f + 0.8f*t)/200.0f; offset = (APPROACHDIST/DOCKTIME)*t + approachfactor*APPROACHDIST; } else if (t <= -DOCKTIME*approachfactor) { offset = 0.0f; } else { offset = approachfactor*APPROACHDIST; } //////////////////////////////////////////////////////////////////////////////// // ADJUST GAINS IF WE ARE FAILING TO LOCK ON TARGET DURING THE FINAL APPROACH //////////////////////////////////////////////////////////////////////////////// // Now truthfully, I'm pretty sure the code below is erroneous. I doubt we are even // calling the ACInCapture function properly. But, the thing is, it actually docks // pretty fast spins so I was too afraid to touch it near the end of week 3. if (t < 0) { state_vector obj, tender; ACGetObjectState(obj); ZRGetMySphState(tender); if (t < -DOCKTIME) { if (!ACInCapture(obj, tender)) { if (endgamegainP < MAXPOSGAIN) { endgamegainP += 0.2; endgamegainD += 0.05; gainfactorA += 0.15; } //DEBUG(("CAP! t=%d P=%3.3f d=%3.3f a=%3.3f\n", t, endgamegainP, endgamegainD, gainfactorA)); } else { DEBUG(("+CAP\n")); } } else if (t < -8) { if (!ACInCone(obj, tender)) { //t++; // we are not synchronized if (endgamegainP < MAXPOSGAIN) { endgamegainP += 0.2; endgamegainD += 0.05; gainfactorA += 0.1; } //DEBUG(("CONE! t=%d P=%3.3f d=%3.3f a=%3.3f\n", t, endgamegainP, endgamegainD, gainfactorA)); } else { DEBUG(("+CONE\n")); } } } /////////////////////////////////////////////////////////////////////////////// // END OF ADJUST GAINS /////////////////////////////////////////////////////////////////////////////// if (t == 1) { // zero out the error term of the "I" gains in both attitude and position // because now we're trying to dock with a potentially fast spinning target // so past errors really have nothing to do with what's about to happen //ctrlPositionPIDinit(); //ctrlAttitudePIDinit(); } if (t <= ADJUSTGAINSTIME) { // increase the gains near the end so we stay on target // this is sufficient to dock with spins up to about 0.05 rad/s // we would probably have to turn gains up even higher to dock with // faster spinning targets. This is an area for further study. ZRSetPosGains(endgamegainP*30*0.0172f,0.0f,endgamegainD*7*0.172f); if (t <= 0) { attgainramp = 1.0f; } else { attgainramp = (ADJUSTGAINSTIME-t)/ADJUSTGAINSTIME; } ZRSetAttGains(gainfactorA*attgainramp*0.0045f, gainfactorA*attgainramp*1.0*0.00018f, gainfactorA*attgainramp*0.5*0.0143f); } // Boost the "I" gain on attitude when we start to capture if (t <= -DOCKTIME*approachfactor) { ZRSetAttGains(gainfactorA*0.0045f, gainfactorA*2.1*0.00018f, gainfactorA*0.5*0.0143f); } // project out in the same direction as the capture area, which is 0.25 m // out from the center of the pod. So take the difference, multiply by 4 // (which changes 0.25 m to 1m) then multiply by the offset desired. // The offset is fixed based on rotation speed up to t == 0, then slowly // decreases to zero after // that so we pull in to the capture area linearly. capState[POS_X] -= 4*offset*(objState[0]-capState[0]); capState[POS_Y] -= 4*offset*(objState[1]-capState[1]); capState[POS_Z] -= 4*offset*(objState[2]-capState[2]); // the velocity out to the projected point is also higher by the same factor // that the offset is larger than the capture area (0.25) capState[VEL_X] *= (offset + 0.25)/0.25; capState[VEL_Y] *= (offset + 0.25)/0.25; capState[VEL_Z] *= (offset + 0.25)/0.25; // reverse the rotation rates as we wish to match the rotation // but in the opposite direction capState[RATE_X] *= -1; capState[RATE_Y] *= -1; capState[RATE_Z] *= -1; ///////////////////////////////////////////////////////////////////////////// // HANDLE THE CASE WHERE THE POD IS IN FRONT OF THE CAPTURE AREA ///////////////////////////////////////////////////////////////////////////// if (t > 20) { float dpod, dcap; state_vector tmp_obj, tmp_cap; ACPredictState(((t < 15)?t:15), tmp, tmp_obj); ACGetCaptureState(tmp_obj, tmp_cap); dpod = mathVecMagnitude(tmp_obj,3); dcap = mathVecMagnitude(tmp_cap,3); //DEBUG(("coldet: dp=%f dc=%f\n", dpod, dcap)); if (needwaypoint && dpod + 0.1 < dcap) { // If the object is above the z axis go under it otherwise go over it // yeah, I know this is not the very best route and I should be doing all // kinds of crazy cross products to find the optimal plane, but practically // speaking, this works pretty well and keeps us in bounds with just a couple // of lines of code. In addition, the new requirement in week 3 of 0.03 rad/sec // minimum y-z spin magnitude more or less guarantees that we will never even // run into this case since the pod will always spin to a position where we can // dock without going around it (unless someone comes up with some tricky puzzle // situation) DEBUG(("**************WP*************\n")); if (tmp_obj[2] > 0) capState[POS_Z] -= 0.49f; else capState[POS_Z] += 0.49f; } } if (t > -DOCKTIME*approachfactor) { float mag; float proj_dist[3]; state_vector tmp_obj, tmp_cap, tmp_tender; ACGetObjectState(tmp_obj); ZRGetMySphState(tmp_tender); ACGetCaptureState(tmp_obj, tmp_cap); DEBUG(("Current distance to pod: %3.3f\n", mathVecMagnitude(tmp_obj, 3))); DEBUG(("Current distance to cap: %3.3f\n", mathVecMagnitude(tmp_cap, 3))); // If we are here then we're heading toward the approach point. // One problem with using very small gains for this part of the journey // is that we may not make it to the approach point at t == 0 as expected. // So here, we estimate if we're going to get there in time, and if not // then slightly bump up the gainfactor. proj_dist[0] = capState[POS_X] - capState[VEL_X]*t; proj_dist[1] = capState[POS_Y] - capState[VEL_Y]*t; proj_dist[2] = capState[POS_Z] - capState[VEL_Z]*t; mag = mathVecMagnitude(capState, 3); DEBUG(("t=%d Projected Dist To Approach Point=%3.3f offset=%3.3f\n", t, mag, offset)); } //////////////////////////////////////////////////////////////////// // CAPTURE PHASE ERROR INSTRUMENTATION // // Some debugging stuff in case we don't lock on to try to figure // out why we're failing to capture. These are plotted out to the // plot tool for analysis. Different reasons for lock failure imply // that different gains or other factors may need to be changed //////////////////////////////////////////////////////////////////// if (offset == 0) { // we should be locked on the capture area float dist, angle, relvel; state_vector obj, tender, cap; float att_pod[3], att_tender[3]; float ref_vec[3]; int good = 0, percent; capStats[3]++; // number of trials DEBUG(("\nCAP T=%d ", t)); ACGetObjectState(obj); ZRGetMySphState(tender); // are we the right distance? dist = mathVecMagnitude(obj, 3); dbg[0] = (0.25-dist)*100; // convert to centimeters if (dbg[0] < 0) dbg[0] *= -1; // gee there is no absf() available? capStats[0] += dbg[0]; if (dist > 0.24 && dist < 0.26) { good++; } DEBUG(("D=%2.2f cm @%d ", dbg[0], good)); // are we the right attitude angle? ref_vec[0] = 1; ref_vec[1] = 0; ref_vec[2] = 0; ZRQuat2AttVec(ref_vec, &obj[QUAT_1], att_pod); ZRQuat2AttVec(ref_vec, &tender[QUAT_1], att_tender); att_tender[0] *= -1; att_tender[1] *= -1; att_tender[2] *= -1; angle = mathVecInner(att_pod, att_tender, 3); angle = acosf(angle)*360/6.28318f; // convert to degrees dbg[1] = angle; capStats[1] += dbg[1]; if (angle < 2.5f) { good++; } DEBUG(("A=%2.2fdg @%d ", angle, good)); // do we have the right relative velocity WRT to the capture area? ACGetCaptureState(obj, cap); relvel = mathVecMagnitude(&cap[VEL_X], 3); // already relative dbg[2] = relvel*1000; // measure this in millimeters capStats[2] += dbg[2]; if (relvel < 0.005) { good++; } dbg[3] = good; if (good == 3) { inCapture++; totInCapture++; } else { inCapture = 0; } DEBUG(("V=%2.2fmm/s @%d CT=%2.2f IN=%d\n", dbg[2], good, capStats[3], inCapture)); percent = 100*totInCapture/capStats[3]; DEBUG(("CAPSTATS AVG: D=%3.1fcm A=%3.1f deg V=%3.1f mm/s %%=%d F=%2.2f\n", capStats[0]/capStats[3], capStats[1]/capStats[3], capStats[2]/capStats[3], percent, ACGetFuelRemaining() )); ZRSetDebug(dbg); } /////// END OF CAPTURE PHASE ERROR INSTRUMENTATION ///////// //Negate the position and velocity to center the frame on the capture //zone. memset(refState,0,sizeof(float)*6); mathVecSubtract(refState, refState, capState, 6); //'refState' now holds the position of the tender with respect to //the capture zone as well as the global attitude of the tender. This //value is used as the measurement for the controller. ZRSetCtrlMeasurement(refState); //=== Create Target State for Controller ===// //Position and velocity are just controlled to 0 since the state //is referenced with respect to the capture zone memset(targState,0,sizeof(float)*6); //Attitude and rotation rates are set to the same values as the capture zone //7 values are copied here (4 quats, 3 rates) // // however, we want to set the quats not to the position of the approach area // but rather the position of the docking point of the actual capture area // so project forward to -DOCKTIME*approachfactor if (t > approachfactor*(-DOCKTIME)) { ACGetObjectState(tmp); ACPredictState(t - (-DOCKTIME)*approachfactor, tmp, objState); ACGetCaptureState(objState, capState); } memcpy(&targState[QUAT_1],&capState[QUAT_1],sizeof(float)*7); //Assign targets ZRSetPositionTarget(&targState[POS_X]); ZRSetVelocityTarget(&targState[VEL_X]); // very near the end start aligning the attitude using quats if (t < 15) { ZRSetQuatTarget(&targState[QUAT_1]); } if (t < 3) { ZRSetAttRateTarget(&targState[RATE_X]); // track the rate of rotation too } //END::PROC::ZRUser } void ZRInit01() { //BEGIN::PROC::ZRInit firstrun = 1; endgamegainP = 1.0f; endgamegainD = 1.0f; approachfactor = 1.0f; needwaypoint = 0; lookahead = 1; gainfactorA = 1.0f; memset(distance,0,sizeof(float)*210); memset(obscured,0,sizeof(int)*210); giveup = 0; t = -1; gainfactor = 1.5f; inCapture = 0; memset(capStats,0,sizeof(float)*4); totInCapture = 0; memset(veldot,0,sizeof(float)*210); memset(inboundsAP,0,sizeof(int)*210); memset(inboundsCP,0,sizeof(int)*210); //END::PROC::ZRInit { //BEGIN::PROC::ZRInit ObjectParams p; //Principal moments of inertia p.inertia[0] = 0.01f; p.inertia[1] = 0.01f; p.inertia[2] = 0.01f; //Initial velocity p.v0[0] = 0.00f; p.v0[1] = -0.005f; // was .005 p.v0[2] = -0.0002f; //Initial position p.xzpos[0] = 0.0f; p.xzpos[1] = 0.0f; //Initial orientation p.q0[0] = 0.7071f; p.q0[1] = 0.7071f; p.q0[2] = 0.0f; p.q0[3] = 0.0f; mathVecNormalize(p.q0,4); //Initial rotation rates p.omega0[0] = 0.04f; p.omega0[1] = 0.08f; p.omega0[2] = 0.08f; if (0) { // test p.omega0[0] = 0.01f; p.omega0[1] = 0.01f; p.omega0[2] = 0.07f; p.v0[1] = 0.0f; p.v0[2] = 0.0f; } ACSetObjectParams(&p); //END::PROC::ZRInit } } //User-defined procedures