/**************************************************************************** * File: trk_tick.c * * * * Copyright 1995 by Loral Advanced Distributed Simulation, Inc. * * * * Loral Advanced Distributed Simulation, Inc. * * 50 Moulton Street * * Cambridge, MA 02138 * * 617-441-2000 * * * * This software was developed by Loral under U. S. Government contracts * * and may be reproduced by or for the U. S. Government pursuant to * * the copyright license under the clause at DFARS 252.227-7013 * * (OCT 1988). * * * * Contents: Tick routine * * Created: Mon Jul 13 1992 * * Author: jesmith * * $Revision$ * * Remarks: * * * ****************************************************************************/ #include "libtrk_local.h" #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include /*common/include/global*/ #include #include #include #include #include #include #include #include #include #define NEARLY_ZERO(x,err) (((x) > -err) && ((x) < err)) #define CLOSE_ENOUGH(x,y) (((x) <= y + 1) && ((x) >= y - 1)) #define SGN(x) ((x) >= 0 ? 1 : -1) /* The amount of time we slide when somebody smashes into us. We go * ahead a compute a collision velocity based on momentum, so this * shouldn't look too fake. */ #define SLIDE_TIME 0.5 /* seconds */ /* Number of milliseconds between checks to see if the hull is within * a hydrology abstract feature. */ #define CHECK_PERIOD 10000 /* milliseconds */ /* The longest tick we trust linear dynamics when using external control. */ #define MAX_INTERNAL_TICK_SECONDS 1000.0 #define PI_4 0.7853990675 /* CCTT Mobility Model X Constants */ #define CCTTX_HEADING_ERROR_RATE_TAU_1 0.8 #define CCTTX_HEADING_ERROR_RATE_TAU_2 0.2 #define CCTTX_MU_COEFF_FACTOR 2.0 #define CCTTX_TANGENT_SLIP_SPEED_CONSTANT 1.0 static int32 first_tick; static HULLS_STATS *trkStats = NULL; static int32 add_wave_influence(int32 vehicle_id, TRACKED_VARS *tracked, float64 dt, int32 cell, float64 position[3], float64 velocity[3], float64 hull_to_world[3][3], CTDB_MES_NODE_KEY mes_key); static void skew(int32 vehicle_id); static void get_soil(int32 vehicle_id, TRACKED_VARS *tracked, CTDB *ctdb); static void flat_2D(float64 x, float64 y, float64 *xout, float64 *yout); static int32 trk_in_trouble_slope( float64 soil_slope, TRACKED_VARS *tracked, float64 position[XYZC], struct tracked_soils *soil); static int32 trk_wet_weather_on( TRACKED_VARS *tracked, float64 position[XYZC]); static int32 trk_simnet_to_cctt_soil_mapping( TRACKED_VARS *tracked, float64 position[XYZC], int32 soil_type); static float64 trk_get_max_speed( int32 cctt_soil); static void tick_mini_driver( int32 vehicle_id, TRACKED_VARS *tracked, float64 dt); static void check_deployed( int32 vehicle_id, float64 *max_mps, struct tracked_soils *soil); static int32 cctt_get_index( float64 value, float64 min, float64 max, int32 n_values); /* This function uses the vehicle ID to check if there is a attachment which * affects vehicle speed. A more expandable implementation way is to have * a second set of "soil speeds" for vehicles with deployables. */ static void check_deployed( int32 vehicle_id, float64 *max_mps, struct tracked_soils *soil) { char *deploy_name[ENT_MAX_PARTS]; int32 comp; int32 is_attached; int32 is_valid; int32 is_active; int32 is_error; int32 type; int32 j, deployed; READER_UNION *speed_table, *soil_table; char whole_name[DEPLOYABLES_MAX_NAME_LENGTH+5] = "part_"; int32 plow_type; uint32 the_default; /* check to see if there is a plow component attached to the entity. * which can either be artic category "plow" or "dirtmover". */ if (ent_get_artics_in_category(vehicle_id, tracked_plow_cat, deploy_name) || ent_get_artics_in_category(vehicle_id, tracked_dirtmover_cat, deploy_name)) { /* special case for the FWMR and moldboard. if the plow is a FWMR * then we really want to get the status of the moldboard that it * is attached to rather than the FWMR. Note that we still use the * FWMR parametric data in this case.. */ if (deploy_name[0] == tracked_fwmr_symbol) comp = cmpnt_locate(vehicle_id, tracked_moldboard_symbol); else comp = cmpnt_locate(vehicle_id, deploy_name[0]); /* get information about plow.. */ DEPLOYABLES_GET_POSITION(vehicle_id, comp, &is_valid, &is_active, &is_error); /* check if it is deployed or stowed */ deployed = (is_valid && is_active); /* get the part number from the name */ strcpy(whole_name+5, deploy_name[0]); plow_type = const_name_to_value(reader_get_symbol(whole_name)); /* adjust the speed depending upon the plow data from the * parametric data for this vehicle.. */ /* look for the plow type.. */ for (j=0; jn_plows; j++) if (soil->plow_type[j] == plow_type) break; /* if the plow type was found, then adjust the speed..*/ if (j < soil->n_plows) { if (*max_mps > soil->plow_data[j][deployed]) *max_mps = soil->plow_data[j][deployed]; } else fprintf(stderr, "libtracked: plow speed data for %s is not " "specified for soil type %d.\n", deploy_name[0], soil->soil_type); } /* now look for bridge components attached to this entity * note that it is possible to have a bridge and a plow, so * we should check both of these.. */ if (ent_get_artics_in_category(vehicle_id,tracked_bridge_cat,deploy_name)) { speed_table = reader_find_tag(deploy_name[0], tracked_speeds, READER_UNTAGGED, READER_NO_ERRORS); if (speed_table && (*max_mps > speed_table[2].real)) *max_mps = speed_table[2].real; } } void tracked_tick( int32 vehicle_id, uint32 interval_since_last_tick, ADDRESS user_data) { TRACKED_VARS *tracked = (TRACKED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), tracked_user_data_handle); float64 position[XYZC]; float64 velocity[XYZC]; float64 direction[XYZC]; float64 dx, dy, odx, ody; float64 rot[XY][XY]; float64 hull_to_world[XYZ][XYZ]; float64 bridge_dir[XYZ]; float64 bridge_pos[XYZ]; int32 still, right_direction; float64 speed, desired_speed, desired_acceleration; struct tracked_soils *soil; int32 new_soil; float64 max_mps, dz; float64 turn_angle, max_turn; float64 dt; float64 cross, dot; float64 slope, temp_vec[2], dist_from_center; PHYSDB_DATA *pdb, *bridge_pdb; uint32 old_appear, dust; float64 temp_z; int32 num_lanes, i; float64 dist, temp_dist; CTDB_MES_NODE_KEY mes_key; int32 mes_key_cell; float64 ccttx_tangent_slip_speed = 0.0; float64 slope_sin, soil_slope, veh_max_speed; int32 cctt_soil, cctt_vehicle_class; int32 cctt_index[10]; CTDB *ctdb = ent_get_ctdb(vehicle_id); int32 cell = ent_get_cell(vehicle_id); if (!tracked) /* Passive error detection */ return; first_tick = (interval_since_last_tick == time_last_simulation_clock); dt = interval_since_last_tick / 1000.0; if (tracked->being_moved) /* We're being towed, don't bother */ /* $$$ add_wave_influence-like function (different RAO due to * towing ) here. */ return; if (!tracked->node_look_fcn(tracked,0,&mes_key_cell,&mes_key)) ent_get_mes_key(vehicle_id); /* Nothing to do if the vehicle is dead */ if (tracked->state == TRACKED_STATE_DEAD) { tracked_sink_add(vehicle_id, tracked, dt); return; } /* Derive speed and direction, if they aren't given explicitly */ if ((tracked->control_state != TRACKED_EXPLICIT) && (tracked->control_state != TRACKED_EXTERNAL)) tick_mini_driver(vehicle_id, tracked, dt); /* Get needed variables.. all will be with respect to the * same cell (which will be position[CELL3D] */ ent_get_position_gcs(vehicle_id, position); ent_get_velocity_gcs(vehicle_id, velocity); ent_get_direction_gcs(vehicle_id, direction); if(trkStats) hulls_statsTickStart(trkStats, vehicle_id, tracked->speed, position, direction, dt); /* Cache whether we are not moving */ still = ((velocity[X] == 0.0) && (velocity[Y] == 0.0)); pdb = ent_get_physdb(vehicle_id); if (!tracked->cells_initted) { /* this is a hack. since we cannot find out the vehicles * current direction in tracked_create like we used to (we * shouldn't be calling another subclass's functions during * our subclasses create routine).. so, we initialize its cell * to -1 in our create routine indicating that it hasn't yet * been initialized.. we check for it here... * * this check will only occur the first tick and everytime * a vehicle leaves a cell to enter a new one, so it isn't * called all too often and isn't all that inefficient.. */ tracked->cells_initted = TRUE; VMAT4_VEC_COPY64(direction, tracked->desired_dir); tracked->node_keys.list_cell[0] = ent_get_cell(vehicle_id); } /* make sure that tracked->desired_dir is with respect to * the entities current cell. */ if (cell != tracked->desired_dir[CELL3D]) { /* perform the conversion in place.. */ gcs_vec_to_vec64((int32)tracked->desired_dir[CELL3D], tracked->desired_dir, (int32)cell, tracked->desired_dir); tracked->desired_dir[CELL3D] = cell; } if (!tracked->node_look_fcn(tracked,0,&mes_key_cell,&mes_key)) { mes_key_cell = ent_get_cell(vehicle_id); mes_key = ent_get_mes_key(vehicle_id); } /* If we are not moving and our position changed, then we have been * teleported somewhere and need to adjust our rotation to be placed * correctly on the terrain. */ if ((still) && (!(CLOSE_ENOUGH(position[X], tracked->saved_position[X]))) || (!(CLOSE_ENOUGH(position[Y], tracked->saved_position[Y]))) || (!(position[CELL3D] == tracked->saved_position[CELL3D]))) { /* Save the z value for possible restoration later */ temp_z = position[Z]; /* clear the node key history list and set the current node * key to be that of the current enclosure. */ mes_key = ent_get_mes_key(vehicle_id); tracked_clear_node_key_list(tracked,mes_key_cell,mes_key); tracked_set_current(tracked,mes_key_cell,mes_key); ctdb_place_vehicle_ml(ctdb, position[X], position[Y], pdb->dimensions[Y], pdb->dimensions[X], pdb->mass_kg, tracked->desired_dir[X], tracked->desired_dir[Y], position+Z, hull_to_world, &new_soil, &mes_key); ent_set_rotation_gcs(vehicle_id, hull_to_world, cell); /* Restore the z for structures, since this may not necessarily * be the z at the center of mass of the structure * which ctdb_place_vehicle returns. Also set soil latch, so * it doesn't perform unnecessary calculations and reset z again. */ if (VTAB_TYPES_MATCH(vtab_vehicle_type(vehicle_id),VTAB_STRUCTURE)) { position[Z] = temp_z; tracked->soil_latch = 0; } /* save our position to that we can see if it changed later.. * (perhaps from a teleport of a vehicle. */ VMAT4_VEC_COPY64(position, tracked->saved_position); } /* Cache whether we are facing the right direction. * Our direction must be parallel (cross = 0) and in the same * direction (dot > 0) as the desired. */ cross = direction[X]*tracked->desired_dir[Y] - direction[Y]*tracked->desired_dir[X]; dot = direction[X]*tracked->desired_dir[X] + direction[Y]*tracked->desired_dir[Y]; right_direction = NEARLY_ZERO(cross,0.001) && (dot > 0.0); /* Grab the desired speed. */ desired_speed = tracked->speed; /* the following section of code determines whether the vehicle * is operating normally or not. If not it will roll to a stop. */ /* Are we out of gas? */ if (tracked->state == TRACKED_STATE_NOGAS) { /* Has some kind soul given us more? */ if (scmp_get_amount(vehicle_id, munition_Fuel, 0) != 0.0) { tracked->state = TRACKED_STATE_HEALTHY; cmpnt_available(vehicle_id, SM_TrackedHull, TRUE); } else { /* If we have no gas, we keep ticking dynamics just * until we stop. */ if (still) return; desired_speed = 0.0; } } /* Are we getting stuck? */ if (tracked->state == TRACKED_STATE_STICKING) { /* Just like running out of gas. Wait till we stop moving. */ if (still) { tracked->state = TRACKED_STATE_STUCK; tracked->stuck_position[X] = position[X]; tracked->stuck_position[Y] = position[Y]; tracked->stuck_position[CELL3D] = position[CELL3D]; cmpnt_available(vehicle_id, SM_TrackedHull, FALSE); return; } } /* Are we stuck? */ if (tracked->state == TRACKED_STATE_STUCK) { /* Have we been towed? */ dx = position[X] - tracked->stuck_position[X]; dy = position[Y] - tracked->stuck_position[Y]; if (!NEARLY_ZERO(dx,0.001) || !NEARLY_ZERO(dy,0.001) || (position[CELL3D] != tracked->stuck_position[CELL3D])) { tracked->state = TRACKED_STATE_HEALTHY; cmpnt_available(vehicle_id, SM_TrackedHull, TRUE); /* Note that we don't know what we're on now. */ tracked->soil_latch = -1; } else return; } if (tracked->state == TRACKED_STATE_DYING) { /* Just like running out of gas. Wait till we stop moving. */ if (still) { if (tracked_mkill_cants_vehicle) skew(vehicle_id); tracked->state = TRACKED_STATE_DEAD; cmpnt_available(vehicle_id, SM_TrackedHull, FALSE); return; } desired_speed = 0.0; } /* If the vehicle is not moving and doesn't want to move, * don't do all this expensive calculation, unless we need to * do a soil lookup (such as on the first tick). */ if ((desired_speed == 0.0) && still && right_direction && (tracked->soil_latch != -1)) { if (scmp_decrement(vehicle_id, munition_Fuel, 0, .00017) == 0.0) { tracked->state = TRACKED_STATE_NOGAS; cmpnt_available(vehicle_id, SM_TrackedHull, FALSE); } (void)add_wave_influence(vehicle_id, tracked, dt, cell, NULL, NULL, NULL, mes_key); return; } /* See if the bridge still exist */ if (vtab_vehicle_type(tracked->bridge_id)) { tracked->bridge_id = 0; tracked->soil_latch = -1; } /* Figure out if we are on a bridge or not. */ if (tracked->bridge_id) { float64 bridge_poly[4][2]; float64 bridge_parallel[2], bridge_perp[2]; ent_get_position_gcscs(tracked->bridge_id, bridge_pos, (int32)position[CELL3D]); /* Compute the polygon which surrounds the bridge. */ bridge_pdb = ent_get_physdb(tracked->bridge_id); ent_get_direction_gcscs(tracked->bridge_id, bridge_dir, (int32)position[CELL3D]); vmat2_unit64(bridge_dir, bridge_parallel); bridge_perp[X] = bridge_parallel[Y]; bridge_perp[Y] = -bridge_parallel[X]; VMAT2_SCAL_VEC_MUL(bridge_pdb->dimensions[Y] * 0.5 + 1.0, bridge_parallel, bridge_parallel); VMAT2_SCAL_VEC_MUL(bridge_pdb->dimensions[X] * 0.5 + 1.0, bridge_perp, bridge_perp); VMAT2_VEC_ADD(bridge_pos, bridge_parallel, bridge_poly[0]); VMAT2_VEC_ADD(bridge_poly[0], bridge_perp, bridge_poly[0]); VMAT2_VEC_SUB(bridge_pos, bridge_parallel, bridge_poly[1]); VMAT2_VEC_ADD(bridge_poly[1], bridge_perp, bridge_poly[1]); VMAT2_VEC_SUB(bridge_pos, bridge_parallel, bridge_poly[2]); VMAT2_VEC_SUB(bridge_poly[2], bridge_perp, bridge_poly[2]); VMAT2_VEC_ADD(bridge_pos, bridge_parallel, bridge_poly[3]); VMAT2_VEC_SUB(bridge_poly[3], bridge_perp, bridge_poly[3]); /* Is the vehicle still on the bridge? */ tracked->on_bridge = geo_is_point_in_convex_polygon(position, bridge_poly, 4) == GEO_PT_INSIDE; if(tracked->on_bridge) { READER_UNION *speeds = reader_find_tag(reader_get_symbol("on_bridge"), tracked_speeds, READER_UNTAGGED, READER_NO_ERRORS); tracked->soil_latch = 0; if (speeds) desired_speed = NS_MIN(desired_speed, speeds[2].real); } if(!tracked->on_bridge && time_last_simulation_clock > tracked->bridge_collision_timeout) { tracked->bridge_id = 0; tracked->soil_latch = -1; } } /* * This section of code determines the speed we are traveling * at if the vehicle is operational. */ /* How fast are we going right now? */ speed = ent_get_speed(vehicle_id); /* Do we want to change gear? */ if (tracked->current_gear != tracked->desired_gear) { if (speed < 0.1) { tracked->current_gear = tracked->desired_gear; if (tracked->current_gear == HULLS_GEAR_FORWARD) tracked->node_look_fcn = (TRACKED_NODE_KEY_LOOK_FCN)tracked_node_key_look_ahead; else tracked->node_look_fcn = (TRACKED_NODE_KEY_LOOK_FCN)tracked_node_key_look_back; } else /* Don't allow gear change until we stop */ desired_speed = 0.0; } /* Make sure we know what soil type we're using */ if (tracked->soil_latch == -1) get_soil(vehicle_id, tracked, ctdb); /* Get the parameters for the soil we're on. */ soil = tracked->params->soils+tracked->soil_latch; /* See if we can't move through this stuff. */ if (soil->max_speeds_mps[TRACKED_MAX_FORWARD] == 0.0) { tracked->state = TRACKED_STATE_STICKING; DEBUG_TRACKED(("Vehicle %d: About to get stuck in soil type %d\n", vehicle_id, soil->soil_type)); } else { /* See if we coasted through */ if (tracked->state == TRACKED_STATE_STICKING) tracked->state = TRACKED_STATE_HEALTHY; } /* Get slope */ slope_sin = direction[Z]; soil_slope = asin(fabs(slope_sin)); soil_slope = RAD_TO_DEG(soil_slope); if (tracked_mobility || tracked_mobility_plus_allow_env) { cctt_soil = trk_simnet_to_cctt_soil_mapping(tracked, position, soil->soil_type); cctt_vehicle_class = 1; cctt_index[0] = cctt_vehicle_class; cctt_soil -= 1; if (cctt_soil < 0) cctt_soil = 0; cctt_index[1] = cctt_soil; veh_max_speed = tracked_cctt_get_table_value(&tracked_cctt_table_max_speed, cctt_index); if (desired_speed > veh_max_speed) desired_speed = veh_max_speed; } if ((tracked_mobility || tracked_mobility_plus_allow_env) && (tracked->state != TRACKED_STATE_START_TROUBLE) && (tracked->state != TRACKED_STATE_IN_TROUBLE)) { if(trk_in_trouble_slope(soil_slope, tracked, position, soil)) { tracked->state = TRACKED_STATE_START_TROUBLE; tracked->waiting_in_trouble_state = time_last_simulation_clock; tracked->saved_desired_speed = desired_speed; } } if (tracked->state == TRACKED_STATE_START_TROUBLE) { if ((tracked->waiting_in_trouble_state + 7000) > time_last_simulation_clock) { desired_speed = 0.0; } else { tracked->state = TRACKED_STATE_IN_TROUBLE; } } if(tracked->state == TRACKED_STATE_IN_TROUBLE) { if (trk_in_trouble_slope(soil_slope, tracked, position, soil)) { if ((tracked->waiting_in_trouble_state + tracked->params->in_trouble_limit) < (time_last_simulation_clock)) { desired_speed = tracked->saved_desired_speed; tracked->state = TRACKED_STATE_STICKING; } } else tracked->state = TRACKED_STATE_HEALTHY; } /* * ModSAF Vehicle Dynmaics Section * * This section computes the ModSAF Mobility Model dynamics. * * Input: * tracked->desired_dir[X],[Y] - desired direction * desired_speed - desired speed * * Output: * dx, dy - direction * speed - speed */ if (tracked->params->mobility_model == TRK_MODSAF_MOBILITY_MODEL) { /* * Compute what the speed will be for this tick. * Note that this is only an approximation, since we haven't yet * determined what direction we'll be facing this tick. However, * the difference will be very small, and this saves significant * time by avoiding an elevation lookup. */ /* How fast can we go on this soil? */ if (tracked->current_gear == HULLS_GEAR_FORWARD) { max_mps = soil->max_speeds_mps[TRACKED_MAX_FORWARD]; dz = direction[Z]; check_deployed(vehicle_id,&max_mps, soil); } else { max_mps = soil->max_speeds_mps[TRACKED_MAX_REVERSE]; dz = -direction[Z]; } /* Adjust for grade */ if(!tracked->on_bridge) if (dz > soil->max_climb_sin) /* Too steep */ max_mps = 0.0; else if (dz > 0.0) /* Not downhill */ max_mps *= 1.0 - sqrt(dz / soil->max_climb_sin); /* Magic */ /* How fast do we want to go? */ if (desired_speed > max_mps) desired_speed = max_mps; /* If we want to change our speed, do as much as possible */ if (speed != desired_speed) { /* How much would we have to accelerate to make this speed? */ desired_acceleration = (desired_speed - speed) / dt; /* First do braking */ if (desired_acceleration < 0.0) { #if 0 /* If we need to brake to avoid collisions it does * not make sense to limit how much we brake. Perhaps * we need to differentiate between emergengy and common * braking. */ /* Only deccelerate as much as allowed */ if ((tracked->max_accel != 0.0) && (-desired_acceleration > tracked->max_accel)) desired_acceleration = -tracked->max_accel; #endif /* Can we slow down that fast? */ if (-desired_acceleration < soil->max_decel_mps2) speed = desired_speed; else speed = speed - soil->max_decel_mps2 * dt; } else { /* Only accelerate as much as allowed */ if ((tracked->max_accel != 0.0) && (desired_acceleration > tracked->max_accel)) desired_acceleration = tracked->max_accel; /* Can we speed up that fast? */ if (desired_acceleration < soil->max_accel_mps2) speed = desired_speed; else speed = speed + soil->max_accel_mps2 * dt; } } /* * Compute the turn accomplished during this tick */ /* Make our desired direction a 2D unit vector */ flat_2D(tracked->desired_dir[X], tracked->desired_dir[Y], &tracked->desired_dir[X], &tracked->desired_dir[Y]); /* Don't do anything expensive if we're already pointing the * right way. */ if (right_direction) { dx = tracked->desired_dir[X]; dy = tracked->desired_dir[Y]; } else { /* Get our current direction in a 2D unit vector */ flat_2D(direction[X], direction[Y], &dx, &dy); /* How much do I want to turn (in radians)? * Both directions are unit vectors, so we can just use the * acos of the dot product to find the angle, and the sign * of the cross product to find left or right. We already * computed the cross product a while ago, and changing the * magnitudes of the vectors did not affect its sign. */ if (cross > 0) /* left */ turn_angle = acos(dx*tracked->desired_dir[X] + dy*tracked->desired_dir[Y]); else turn_angle = -acos(dx*tracked->desired_dir[X] + dy*tracked->desired_dir[Y]); /* Can I turn that much? Compare to the lesser of what * I can do and what I am allowed to do. * Zero in tracked->max_turn means no limit from driver. */ if ((tracked->max_turn == 0.0) || (soil->max_turn_rps < tracked->max_turn)) max_turn = soil->max_turn_rps * dt; else max_turn = tracked->max_turn * dt; if ((turn_angle > -max_turn) && (turn_angle < max_turn)) { /* Adopt the new direction. */ dx = tracked->desired_dir[X]; dy = tracked->desired_dir[Y]; } else { /* Turn the maximum amount */ if (turn_angle < 0) max_turn = -max_turn; vmat2_primary_rotation64(sin(max_turn), cos(max_turn), rot); odx = dx, ody = dy; dx = odx * rot[X][X] + ody * rot[Y][X]; dy = odx * rot[X][Y] + ody * rot[Y][Y]; } } } /* * CCTT Vehicle Dynmaics Section * * This section of code attempts to replicate as closely as possible * the dynamics algorithm as implementation in ADA in the CCTT Program. * Every attempt was made to match algorithm flow, variable names and * coding styles. However, where ModSAF variables exist --- such as * speed, direction, velocity, etc. --- they were used in place of the * CCTT names. * * Input: * tracked->desired_dir[X],[Y] - desired direction * desired_speed - desired speed * * Output: * dx, dy - direction * speed - speed * */ else if (tracked->params->mobility_model == TRK_CCTT_MOBILITY_MODEL) { float64 cctt_slope; /* slope (radians) */ float64 cctt_slope_sin; /* sine of slope */ float64 cctt_slope_cos; /* cosine of slope */ int32 cctt_vehicle_class; /* CCTT vehicle class */ int32 cctt_soil_type; /* CCTT soil type */ int32 cctt_index[10]; /* temporary index array */ int32 cctt_local_gear; /* local gear (FORWARD, REVERSE or STOP) */ float64 cctt_local_commanded_speed; /* commanded speed (meters/second) */ float64 cctt_heading_error; /* heading error (radians +/- PI) */ float64 cctt_heading_error_rate; /* heading error rate (radius/second) */ float64 cctt_speed_error; /* difference in desired and actual speed (meters/second) */ float64 cctt_speed_error_limited_abs; /* limited, absolute value of speed error */ float64 cctt_brake_pos_percent; /* brake position percent (from 0.0 to 1.0) */ float64 cctt_brake_linear_factor; /* brake linear factor (TABLE LOOKUP) */ float64 cctt_commanded_throttle_pos_rate; /* desired throttle position rate (from 0.0 to 1.0) */ float64 cctt_accel_mod; /* modified acceleration value */ float64 cctt_force_drive; /* driving force (newtons) */ float64 cctt_force_brake; /* brake force (newtons) */ float64 cctt_force_friction; /* frictional force (newtons) */ float64 cctt_force; /* sum of forces on the vehicle (newtons) */ float64 cctt_weight_longitudinal; /* weight along direction of motion (newtons) */ float64 cctt_weight_vertical; /* weight normal to direction of motion (newtons) */ float64 cctt_mu_eff_coeff; /* effective coefficient of friction */ float64 cctt_max_turn_rate; /* maximum turn rate (radians/second) (TABLE LOOKUP) */ float64 cctt_max_speed; /* maximum speed (meters/second) (TABLE LOOKUP) */ ENV_INTERFACE cctt_env_interface; /* environmental interface */ CTDB_ABSTRACT_DATA *data; /* CCTT Initialize values */ /* Get slope */ cctt_slope_sin = direction[Z]; cctt_slope = asin(cctt_slope_sin); cctt_slope_cos = cos(cctt_slope); /* CCTT vehicle class */ cctt_index[0] = (int32) ent_get_vehicle_class(vehicle_id); cctt_vehicle_class = (int32) tracked_cctt_get_table_value(&tracked_cctt_table_vehicle_map, cctt_index); /* CCTT soil type */ /* CCTT soil type */ /* The CCTT soil type indices are assigned as follows: * * cctt_index[0] - subsurface saturation * cctt_index[1] - surface wetness * cctt_index[2] - soil type */ if(time_last_simulation_clock - tracked->last_check_time > CHECK_PERIOD) { tracked->last_check_time = time_last_simulation_clock; tracked->within_hydro_surface = ctdb_point_within_abstract(ctdb, CTDB_ABSTRACT_HYDRO_SURFACE, position[X], position[Y], &data) && data->hydro_surface.wetness; tracked->within_hydro_subsurface = ctdb_point_within_abstract(ctdb, CTDB_ABSTRACT_HYDRO_SUBSURFACE, position[X], position[Y], &data) && data->hydro_subsurface.wetness; } if(tracked->within_hydro_subsurface) cctt_index[0] = 1; else cctt_index[0] = 0; /* Get precipitation information */ /* Set environmental interface structure */ /* Set location */ VMAT4_VEC_COPY64(position, cctt_env_interface.u.generic_vector_to_value. input_vector); if (env_get(ENV_PRECIPITATION_RATE, &cctt_env_interface) == -1 && !tracked->within_hydro_surface) cctt_index[1] = 0; else { /* if the precipitation rate is positive use the CCTT wet soil * types */ if (cctt_env_interface.u.precipitation_rate.output_value > 0.0 || tracked->within_hydro_surface) cctt_index[1] = 1; else cctt_index[1] = 0; } /* Map ModSAF to CCTT Soil Types */ cctt_index[2] = (int32) soil->soil_type; cctt_soil_type = (int32) tracked_cctt_get_table_value(&tracked_cctt_table_soil_map, cctt_index); /* CCTT soil types are indexed from 1 to 30. However, since we index * into the data tables from 0, we must subtract one from this index. * A 0 CCTT value means soil type unknown, in this case we print a * warning and assign 0 to CCTT type 1 */ cctt_soil_type -= 1; if (cctt_soil_type < 0) cctt_soil_type = 0; /* CCTT Controller Section */ /* Read maximum speed (meters/second) based on (1) Vehicle Class and * (2) Soil Type from CCTT data table */ cctt_index[0] = cctt_vehicle_class; cctt_index[1] = cctt_soil_type; cctt_max_speed = tracked_cctt_get_table_value( &tracked_cctt_table_max_speed, cctt_index); /* Determine Local Gear */ cctt_local_gear = tracked->current_gear; /* Determine Commanded Speed */ cctt_local_commanded_speed = desired_speed; if (cctt_local_commanded_speed > cctt_max_speed) cctt_local_commanded_speed = cctt_max_speed; /* Project direction vectors into the xy plane */ flat_2D(tracked->desired_dir[X], tracked->desired_dir[Y], &tracked->desired_dir[X], &tracked->desired_dir[Y]); flat_2D(direction[X], direction[Y], &dx, &dy); /* Determine Heading Error */ cctt_heading_error = vmat2e_angle_between_vectors64(dx, dy, tracked->desired_dir[X], tracked->desired_dir[Y]); /* Determine Heading Error Rate */ cctt_heading_error_rate = tracked->params->heading_error_rate_tau* cctt_heading_error; /* Read Maximum Turn Rate (radians/second) based on (1) Vehicle Class * and (2) Soil Type from CCTT data tables */ cctt_index[0] = cctt_vehicle_class; cctt_index[1] = cctt_soil_type; cctt_max_turn_rate = tracked_cctt_get_table_value( &tracked_cctt_table_max_turn_rate, cctt_index); /* Limit Heading Error Rate */ if (cctt_heading_error_rate > cctt_max_turn_rate) cctt_heading_error_rate = cctt_max_turn_rate; else if (cctt_heading_error_rate < -cctt_max_turn_rate) cctt_heading_error_rate = -cctt_max_turn_rate; /* Determine Turn Angle */ turn_angle = cctt_heading_error_rate * dt; /* Compute New Angle */ vmat2_primary_rotation64(sin(turn_angle), cos(turn_angle), rot); odx = dx, ody = dy; dx = odx * rot[X][X] + ody * rot[Y][X]; dy = odx * rot[X][Y] + ody * rot[Y][Y]; /* Determine Speed Error */ cctt_speed_error = cctt_local_commanded_speed - speed; /* Limit Speed Error */ if (fabs(cctt_speed_error) > cctt_max_speed) cctt_speed_error_limited_abs = cctt_max_speed; else cctt_speed_error_limited_abs = fabs(cctt_speed_error); /* Apply Brakes */ if (cctt_speed_error < 0.0) { if (cctt_max_speed > 0.0) cctt_brake_pos_percent = 1.0 - exp(-fabs(cctt_speed_error_limited_abs)/cctt_max_speed); else cctt_brake_pos_percent = 1.0; tracked->commanded_throttle_pos = 0.0; } /* Apply Throttle */ else { /* compute modified acceleration value */ cctt_accel_mod = tracked->acceleration; /* compute commanded throttle position rate */ cctt_commanded_throttle_pos_rate = tracked->params->throttle_forward_loop_gain* (SGN(cctt_speed_error)*cctt_speed_error_limited_abs - tracked->params->throttle_accel_feedback_gain*cctt_accel_mod); /* integrate throttle position */ tracked->commanded_throttle_pos += cctt_commanded_throttle_pos_rate*dt; /* limit commanded throttle position */ if (tracked->commanded_throttle_pos > 1.0) tracked->commanded_throttle_pos = 1.0; else if (tracked->commanded_throttle_pos < 0.0) tracked->commanded_throttle_pos = 0.0; cctt_brake_pos_percent = 0.0; } /* compute throttle position */ if (cctt_local_commanded_speed > 0.0) tracked->throttle_pos += tracked->params->tau_throttle_inv* dt*(tracked->commanded_throttle_pos - tracked->throttle_pos); else tracked->throttle_pos = 0.0; /* limit throttle position */ if (tracked->throttle_pos > 1.0) tracked->throttle_pos = 1.0; else if (tracked->throttle_pos < 0.0) tracked->throttle_pos = 0.0; /* CCTT Dynamics Section */ /* Determine Driving Force */ cctt_index[0] = cctt_vehicle_class; cctt_index[1] = cctt_soil_type; cctt_index[2] = cctt_get_index(tracked->throttle_pos, 0.0, 1.0, tracked->params->n_throttle_positions); cctt_index[3] = cctt_get_index(cctt_slope, 0.0, PI_4, tracked->params->n_slopes); cctt_index[4] = cctt_get_index(speed, 0.0, cctt_max_speed, tracked->params->n_speeds); cctt_force_drive = TRACKED_CCTT_GRAVITY* tracked_cctt_get_table_value(&tracked_cctt_table_force_drive, cctt_index); /* Scale drive force for reverse direction */ if (cctt_local_gear == TRACKED_CCTT_GEAR_REVERSE) cctt_force_drive *= tracked->params->reverse_drive_factor; /* Determine Braking Force */ if ((speed > TRACKED_CCTT_THRESHOLD) && (cctt_brake_pos_percent > TRACKED_CCTT_THRESHOLD)) { cctt_index[0] = cctt_vehicle_class; cctt_index[1] = cctt_soil_type; cctt_brake_linear_factor = tracked_cctt_get_table_value( &tracked_cctt_table_force_brake, cctt_index); cctt_force_brake = pdb->mass_kg * cctt_brake_pos_percent * cctt_brake_linear_factor; cctt_force_brake = tracked_cctt_get_table_value( &tracked_cctt_table_force_brake, cctt_index); } else cctt_force_brake = 0.0; /* Determine Gravitational Force */ cctt_weight_longitudinal = pdb->mass_kg*TRACKED_CCTT_GRAVITY* cctt_slope_sin; cctt_weight_vertical = pdb->mass_kg*TRACKED_CCTT_GRAVITY* cctt_slope_cos; /* Ignore Gravity Going Downhill */ if (((cctt_slope < 0.0) && (cctt_local_gear == TRACKED_CCTT_GEAR_FORWARD)) || ((cctt_slope > 0.0) && (cctt_local_gear == TRACKED_CCTT_GEAR_REVERSE))) { cctt_weight_longitudinal = 0.0; cctt_weight_vertical = pdb->mass_kg*TRACKED_CCTT_GRAVITY; } /* Determine Friction Force */ cctt_index[0] = cctt_vehicle_class; cctt_index[1] = cctt_soil_type; cctt_mu_eff_coeff = tracked_cctt_get_table_value( &tracked_cctt_table_friction, cctt_index); cctt_force_friction = cctt_mu_eff_coeff*cctt_weight_vertical; if (NEARLY_ZERO(speed, TRACKED_CCTT_THRESHOLD)) { cctt_force_friction = 0.0; cctt_weight_longitudinal = 0.0; } /* Sum Forces */ if (cctt_local_gear == TRACKED_CCTT_GEAR_FORWARD) cctt_force = cctt_force_drive - cctt_weight_longitudinal - cctt_force_friction - cctt_force_brake; else if (cctt_local_gear == TRACKED_CCTT_GEAR_REVERSE) cctt_force = -cctt_force_drive - cctt_weight_longitudinal + cctt_force_friction + cctt_force_brake; else cctt_force = 0.0; /* Compute Acceleration */ tracked->acceleration = cctt_force/pdb->mass_kg; /* Compute Speed */ if (cctt_local_gear == TRACKED_CCTT_GEAR_FORWARD) speed += tracked->acceleration*dt; else speed -= tracked->acceleration*dt; speed = fabs(speed); if (NEARLY_ZERO(speed, TRACKED_CCTT_THRESHOLD)) speed = 0.0; else if (speed > cctt_max_speed) speed = cctt_max_speed; } /* * CCTT Vehicle Dynmaics X Section * * This section of code attempts to replicate as closely as possible * the dynamics algorithm as implementation in ADA in the CCTT Program * with extensions for rotational inertia and tangential sliding. * Every attempt was made to match algorithm flow, variable names and * coding styles. However, where ModSAF variables exist --- such as * speed, direction, velocity, etc. --- they were used in place of the * CCTT names. * * Input: * tracked->desired_dir[X],[Y] - desired direction * desired_speed - desired speed * * Output: * dx, dy - direction * speed - speed * */ else { float64 cctt_slope; /* slope (radians) */ float64 cctt_slope_sin; /* sine of slope */ float64 cctt_slope_cos; /* cosine of slope */ int32 cctt_vehicle_class; /* CCTT vehicle class */ int32 cctt_soil_type; /* CCTT soil type */ int32 cctt_index[10]; /* temporary index array */ int32 cctt_local_gear; /* local gear (FORWARD, REVERSE or STOP) */ float64 cctt_local_commanded_speed; /* commanded speed (meters/second) */ float64 cctt_heading_error; /* heading error (radians +/- PI) */ float64 cctt_heading_error_accel; /* heading error acceleration (radians/second^2) */ float64 cctt_speed_error; /* difference in desired and actual speed (meters/second) */ float64 cctt_speed_error_limited_abs; /* limited, absolute value of speed error */ float64 cctt_brake_pos_percent; /* brake position percent (from 0.0 to 1.0) */ float64 cctt_brake_linear_factor; /* brake linear factor (TABLE LOOKUP) */ float64 cctt_commanded_throttle_pos_rate; /* desired throttle position rate (from 0.0 to 1.0) */ float64 cctt_accel_mod; /* modified acceleration value */ float64 cctt_force_drive; /* driving force (newtons) */ float64 cctt_force_brake; /* brake force (newtons) */ float64 cctt_force_friction; /* frictional force (newtons) */ float64 cctt_force; /* sum of forces on the vehicle (newtons) */ float64 cctt_weight_longitudinal; /* weight along direction of motion (newtons) */ float64 cctt_weight_vertical; /* weight normal to direction of motion (newtons) */ float64 cctt_mu_eff_coeff; /* effective coefficient of friction */ float64 cctt_max_turn_rate; /* maximum turn rate (radians/second) (TABLE LOOKUP) */ float64 cctt_max_speed; /* maximum speed (meters/second) (TABLE LOOKUP) */ ENV_INTERFACE cctt_env_interface; /* environmental interface */ float64 ccttx_tangent_angle; /* tangent angle (used to determine tangential slipping) */ float64 ccttx_tangent_angle_sin; float64 ccttx_tangent_angle_cos; float64 ccttx_tangent_angle_tan; CTDB_ABSTRACT_DATA *data; /* CCTT Initialize values */ /* Get physical parameters */ pdb = ent_get_physdb(vehicle_id); /* Get slope */ cctt_slope_sin = direction[Z]; cctt_slope = asin(cctt_slope_sin); cctt_slope_cos = cos(cctt_slope); /* CCTT vehicle class */ cctt_index[0] = (int32) ent_get_vehicle_class(vehicle_id); cctt_vehicle_class = (int32) tracked_cctt_get_table_value( &tracked_cctt_table_vehicle_map, cctt_index); /* CCTT soil type */ /* The CCTT soil type indices are assigned as follows: * * cctt_index[0] - subsurface saturation * cctt_index[1] - surface wetness * cctt_index[2] - soil type */ if(time_last_simulation_clock - tracked->last_check_time > CHECK_PERIOD) { tracked->last_check_time = time_last_simulation_clock; tracked->within_hydro_surface = ctdb_point_within_abstract(ctdb, CTDB_ABSTRACT_HYDRO_SURFACE, position[X], position[Y], &data) && data->hydro_surface.wetness; tracked->within_hydro_subsurface = ctdb_point_within_abstract(ctdb, CTDB_ABSTRACT_HYDRO_SUBSURFACE, position[X], position[Y], &data) && data->hydro_subsurface.wetness; } if(tracked->within_hydro_subsurface) cctt_index[0] = 1; else cctt_index[0] = 0; /* Get precipitation information */ /* Set environmental interface structure */ /* Set location */ VMAT4_VEC_COPY64(position, cctt_env_interface.u.generic_vector_to_value. input_vector); if (env_get(ENV_PRECIPITATION_RATE, &cctt_env_interface) == -1 && !tracked->within_hydro_surface) cctt_index[1] = 0; else { /* if the precipitation rate is positive use the CCTT wet soil * types */ if (cctt_env_interface.u.precipitation_rate.output_value > 0.0 || tracked->within_hydro_surface) cctt_index[1] = 1; else cctt_index[1] = 0; } /* Map ModSAF to CCTT Soil Types */ cctt_index[2] = (int32) soil->soil_type; cctt_soil_type = (int32) tracked_cctt_get_table_value(&tracked_cctt_table_soil_map, cctt_index); /* CCTT soil types are indexed from 1 to 30. However, since we index * into the data tables from 0, we must subtract one from this index. * A 0 CCTT value means soil type unknown, in this case we print a * warning and assign 0 to CCTT type 1 */ cctt_soil_type -= 1; if (cctt_soil_type < 0) cctt_soil_type = 0; /* CCTT Controller Section */ /* Read maximum speed (meters/second) based on (1) Vehicle Class and * (2) Soil Type from CCTT data table */ cctt_index[0] = cctt_vehicle_class; cctt_index[1] = cctt_soil_type; cctt_max_speed = tracked_cctt_get_table_value( &tracked_cctt_table_max_speed, cctt_index); /* Determine Local Gear */ cctt_local_gear = tracked->current_gear; /* Determine Commanded Speed */ cctt_local_commanded_speed = desired_speed; if (cctt_local_commanded_speed > cctt_max_speed) cctt_local_commanded_speed = cctt_max_speed; /* Project direction vectors into the xy plane */ flat_2D(tracked->desired_dir[X], tracked->desired_dir[Y], &tracked->desired_dir[X], &tracked->desired_dir[Y]); flat_2D(direction[X], direction[Y], &dx, &dy); /* Determine Heading Error */ cctt_heading_error = vmat2e_angle_between_vectors64(dx, dy, tracked->desired_dir[X], tracked->desired_dir[Y]); /* Determine heading error acceleration */ cctt_heading_error_accel = -CCTTX_HEADING_ERROR_RATE_TAU_1*tracked->ccttx_heading_error_rate - CCTTX_HEADING_ERROR_RATE_TAU_2*cctt_heading_error; /* Determine heading error rate */ tracked->ccttx_heading_error_rate += cctt_heading_error_accel*dt; /* Read Maximum Turn Rate (radians/second) based on (1) Vehicle Class * and (2) Soil Type from CCTT data tables */ cctt_index[0] = cctt_vehicle_class; cctt_index[1] = cctt_soil_type; cctt_max_turn_rate = tracked_cctt_get_table_value( &tracked_cctt_table_max_turn_rate, cctt_index); /* Limit Heading Error Rate */ if (tracked->ccttx_heading_error_rate > cctt_max_turn_rate) tracked->ccttx_heading_error_rate = cctt_max_turn_rate; else if (tracked->ccttx_heading_error_rate < -cctt_max_turn_rate) tracked->ccttx_heading_error_rate = -cctt_max_turn_rate; /* Determine Turn Angle */ turn_angle = -tracked->ccttx_heading_error_rate * dt; /* Compute New Angle */ vmat2_primary_rotation64(sin(turn_angle), cos(turn_angle), rot); odx = dx, ody = dy; dx = odx * rot[X][X] + ody * rot[Y][X]; dy = odx * rot[X][Y] + ody * rot[Y][Y]; /* Determine Speed Error */ cctt_speed_error = cctt_local_commanded_speed - speed; /* Limit Speed Error */ if (fabs(cctt_speed_error) > cctt_max_speed) cctt_speed_error_limited_abs = cctt_max_speed; else cctt_speed_error_limited_abs = fabs(cctt_speed_error); /* Apply Brakes */ if (cctt_speed_error < 0.0) { if (cctt_max_speed > 0.0) cctt_brake_pos_percent = 1.0 - exp(-fabs(cctt_speed_error_limited_abs)/cctt_max_speed); else cctt_brake_pos_percent = 1.0; tracked->commanded_throttle_pos = 0.0; } /* Apply Throttle */ else { /* compute modified acceleration value */ cctt_accel_mod = tracked->acceleration; /* compute commanded throttle position rate */ cctt_commanded_throttle_pos_rate = tracked->params->throttle_forward_loop_gain* (SGN(cctt_speed_error)*cctt_speed_error_limited_abs - tracked->params->throttle_accel_feedback_gain*cctt_accel_mod); /* integrate throttle position */ tracked->commanded_throttle_pos += cctt_commanded_throttle_pos_rate*dt; /* limit commanded throttle position */ if (tracked->commanded_throttle_pos > 1.0) tracked->commanded_throttle_pos = 1.0; else if (tracked->commanded_throttle_pos < 0.0) tracked->commanded_throttle_pos = 0.0; cctt_brake_pos_percent = 0.0; } /* compute throttle position */ if (cctt_local_commanded_speed > 0.0) tracked->throttle_pos += tracked->params->tau_throttle_inv* dt*(tracked->commanded_throttle_pos - tracked->throttle_pos); else tracked->throttle_pos = 0.0; /* limit throttle position */ if (tracked->throttle_pos > 1.0) tracked->throttle_pos = 1.0; else if (tracked->throttle_pos < 0.0) tracked->throttle_pos = 0.0; /* CCTT Dynamics Section */ /* Determine Driving Force */ cctt_index[0] = cctt_vehicle_class; cctt_index[1] = cctt_soil_type; cctt_index[2] = cctt_get_index(tracked->throttle_pos, 0.0, 1.0, tracked->params->n_throttle_positions); cctt_index[3] = cctt_get_index(cctt_slope, 0.0, PI_4, tracked->params->n_slopes); cctt_index[4] = cctt_get_index(speed, 0.0, cctt_max_speed, tracked->params->n_speeds); cctt_force_drive = TRACKED_CCTT_GRAVITY* tracked_cctt_get_table_value(&tracked_cctt_table_force_drive, cctt_index); /* Scale drive force for reverse direction */ if (cctt_local_gear == TRACKED_CCTT_GEAR_REVERSE) cctt_force_drive *= tracked->params->reverse_drive_factor; /* Determine Braking Force */ if ((speed > TRACKED_CCTT_THRESHOLD) && (cctt_brake_pos_percent > TRACKED_CCTT_THRESHOLD)) { cctt_index[0] = cctt_vehicle_class; cctt_index[1] = cctt_soil_type; cctt_brake_linear_factor = tracked_cctt_get_table_value( &tracked_cctt_table_force_brake, cctt_index); cctt_force_brake = pdb->mass_kg * cctt_brake_pos_percent * cctt_brake_linear_factor; cctt_force_brake = tracked_cctt_get_table_value( &tracked_cctt_table_force_brake, cctt_index); } else cctt_force_brake = 0.0; /* Determine Gravitational Force */ cctt_weight_longitudinal = pdb->mass_kg* TRACKED_CCTT_GRAVITY*cctt_slope_sin; cctt_weight_vertical = pdb->mass_kg* TRACKED_CCTT_GRAVITY*cctt_slope_cos; /* Ignore Gravity Going Downhill */ if (((cctt_slope < 0.0) && (cctt_local_gear == TRACKED_CCTT_GEAR_FORWARD)) || ((cctt_slope > 0.0) && (cctt_local_gear == TRACKED_CCTT_GEAR_REVERSE))) { cctt_weight_longitudinal = 0.0; cctt_weight_vertical = pdb->mass_kg*TRACKED_CCTT_GRAVITY; } /* Determine Friction Force */ cctt_index[0] = cctt_vehicle_class; cctt_index[1] = cctt_soil_type; cctt_mu_eff_coeff = tracked_cctt_get_table_value( &tracked_cctt_table_friction, cctt_index); cctt_force_friction = cctt_mu_eff_coeff*cctt_weight_vertical; if (NEARLY_ZERO(speed, TRACKED_CCTT_THRESHOLD)) { cctt_force_friction = 0.0; cctt_weight_longitudinal = 0.0; } /* Sum Forces */ if (cctt_local_gear == TRACKED_CCTT_GEAR_FORWARD) cctt_force = cctt_force_drive - cctt_weight_longitudinal - cctt_force_friction - cctt_force_brake; else if (cctt_local_gear == TRACKED_CCTT_GEAR_REVERSE) cctt_force = -cctt_force_drive - cctt_weight_longitudinal + cctt_force_friction + cctt_force_brake; else cctt_force = 0.0; /* Compute Acceleration */ tracked->acceleration = cctt_force/pdb->mass_kg; /* Compute Speed */ if (cctt_local_gear == TRACKED_CCTT_GEAR_FORWARD) speed += tracked->acceleration*dt; else speed -= tracked->acceleration*dt; speed = fabs(speed); if (NEARLY_ZERO(speed, TRACKED_CCTT_THRESHOLD)) speed = 0.0; else if (speed > cctt_max_speed) speed = cctt_max_speed; /* Tangential sliding */ ccttx_tangent_angle_sin = tracked->ccttx_sin_tangent_angle; ccttx_tangent_angle = asin(ccttx_tangent_angle_sin); ccttx_tangent_angle_cos = cos(ccttx_tangent_angle); ccttx_tangent_angle_tan = ccttx_tangent_angle_sin / ccttx_tangent_angle_cos; if (ccttx_tangent_angle_tan < -CCTTX_MU_COEFF_FACTOR*cctt_mu_eff_coeff) { ccttx_tangent_slip_speed = CCTTX_TANGENT_SLIP_SPEED_CONSTANT; } else if (ccttx_tangent_angle_tan > CCTTX_MU_COEFF_FACTOR*cctt_mu_eff_coeff) { ccttx_tangent_slip_speed = -CCTTX_TANGENT_SLIP_SPEED_CONSTANT; } else { ccttx_tangent_slip_speed = 0.0; } } /* * Figure out what dust to generate */ if (speed >= soil->dust_speeds_mps[2]) dust = vehDustCloudLarge; else if (speed >= soil->dust_speeds_mps[1]) dust = vehDustCloudMedium; else if (speed >= soil->dust_speeds_mps[0]) dust = vehDustCloudSmall; else dust = vehDustCloudNone; /* * Update the vehicle's state given its new velocity */ /* Update position */ if (tracked->current_gear == HULLS_GEAR_REVERSE) { speed = -speed; dust = 0; } if (!still) { /* If we have a desired position, and the tick was kind of * long. Use the externally provided position instead. */ if ((tracked->control_state == TRACKED_EXTERNAL) && (dt > MAX_INTERNAL_TICK_SECONDS)) { VMAT2_VEC_COPY64(tracked->position, position); position[CELL3D] = tracked->position[CELL3D]; } else { position[X] += speed * direction[X] * dt; position[Y] += speed * direction[Y] * dt; if (tracked->params->mobility_model == TRK_CCTT_MOBILITY_MODEL_X) { position[X] += ccttx_tangent_slip_speed*direction[Y]*dt; position[Y] -= ccttx_tangent_slip_speed*direction[X]*dt; } } } /* If the vehicle is on the bridge, get its position and rotation * matrix from the bridge instead of the ground. */ if (tracked->on_bridge) { float64 x_axis[3], y_axis[3], theta; tracked->bridge_collision_timeout = time_last_simulation_clock; ent_get_rotation_gcs(tracked->bridge_id, hull_to_world, NULL); /* We must now construct a rotation matrix for the vehicle that * has the same z-axis as the bridge, and points in the direction * of the vehicle. We have x and y for this direction, we just * need to find z. * * NOTE: This method of calculating the rotation matrix of the * vehicle assumes that the roll angle of the bridge is * nearly zero (a pretty safe assumption for bridges). */ y_axis[X] = dx; y_axis[Y] = dy; y_axis[Z] = 0; vmat2_unit64(y_axis, y_axis); /* Find the angle between the outward normal of the bridge and * (0, 0, 1). */ if(hull_to_world[Z][Z] > 1.0) theta = 0.0; else theta = acos(hull_to_world[Z][Z]); /* Find whether we're sloping up or down */ if(vmat2_dot_prod64(y_axis, hull_to_world[Z]) > 0) theta = -theta; /* Compute the z-component of the vehicle's y-axis in world * coordinates. */ y_axis[Z] = tan(theta); vmat3_unit64(y_axis, y_axis); hull_to_world[Y][X] = y_axis[X]; hull_to_world[Y][Y] = y_axis[Y]; hull_to_world[Y][Z] = y_axis[Z]; /* Now that we have the y- and z-axes of the vehicle's rotation * matrix (i.e. the 2nd and 3rd rows), we can get the x-axis * by taking the cross product of the y- and z-axes. */ VMAT3E_CROSS_PROD(hull_to_world[Y][X], hull_to_world[Y][Y], hull_to_world[Y][Z], hull_to_world[Z][X], hull_to_world[Z][Y], hull_to_world[Z][Z], x_axis); hull_to_world[X][X] = x_axis[X]; hull_to_world[X][Y] = x_axis[Y]; hull_to_world[X][Z] = x_axis[Z]; /* The Z of the bridge is that at the center of the bridge. The * real Z will depend on where on the bridge the vehicle lies. */ /* First, find the slope of the bridge. */ if(bridge_dir[Z] != 1.0) slope = bridge_dir[Z] / (1.0 - bridge_dir[Z] * bridge_dir[Z]); else slope = 0.0; /* `dist_from_center' is the distance the center of the vehicle * is AHEAD of the center of the bridge. If this number is * negative, then the vehicle hasn't reached the center of the * bridge yet. */ /* First, find the magnitude of the distance. */ VMAT2_VEC_SUB(position, bridge_pos, temp_vec); dist_from_center = sqrt(temp_vec[X] * temp_vec[X] + temp_vec[Y] * temp_vec[Y]); /* Next, find out whether the vehicle has passed the center * or not. */ if(VMAT2_DOT_PROD(bridge_dir, temp_vec) < 0.0) dist_from_center = -dist_from_center; position[Z] = bridge_pos[Z] + slope * dist_from_center + bridge_pdb->dimensions[Z]; position[Z] += pdb->model_base_adjustment; new_soil = 0; } else { /* Generate the hull to world matrix. Add a little to the Z seed, * so that we don't fall through multi-level terrain. The vehicle * height is added to the Z seed because it is the largest value which * is safe to add, since vehicles can't travel under overpasses which * are lower than the top of the vehicle. This should hopefully work * when vehicles are traveling up or down steep multi-level * microterrain slopes. */ /* if (direction[Z] > 0.0) position[Z] += direction[Z] * speed * dt; */ int32 index=1, tmp_mes_key_cell, saved_cell, cell; CTDB_MES_NODE_KEY saved, tmp_mes_key, tmp_mes_id; CTDB *tmp_mes_key_ctdb; float64 saved_z; cell = ctdb_get_cell_id(ctdb); saved_z = position[Z] += pdb->dimensions[Z]; /* If the current node key is outside then check if there is another * node key in the list. If so, then check if it is a valid mes key. * if it is try see if the current location is in the MES' footprint. * If so then try and place in that MES. */ if ((mes_key == CTDB_INVALID_MES_NODE_KEY) && tracked->node_look_fcn(tracked,1,&tmp_mes_key_cell,&tmp_mes_key) && tmp_mes_key_cell && ctdb_mes_valid_node_key((tmp_mes_key_ctdb = gcs_get_tdb(tmp_mes_key_cell, FALSE)), tmp_mes_key) && ctdb_mes_find_mes_from_xy(tmp_mes_key_ctdb, position[X], position[Y], 1, &tmp_mes_id) && (CTDB_MES_NODE_KEY_TO_MES_ID(tmp_mes_key) == CTDB_MES_NODE_KEY_TO_MES_ID(tmp_mes_id)) && (tmp_mes_key_cell == cell)) { mes_key_cell = tmp_mes_key_cell; mes_key = tmp_mes_key; } /* First check the current MES key (that libentity has) to * see if this was the proper enclosure. */ saved_cell = mes_key_cell; saved = mes_key; ctdb_place_vehicle_ml(ctdb, position[X], position[Y], pdb->dimensions[Y], pdb->dimensions[X], pdb->mass_kg, tracked->desired_dir[X], tracked->desired_dir[Y], &position[Z], hull_to_world, &new_soil, &mes_key); /* If this was not the correct enclosure start looking ahead in * the list to see if the next enclosure (...) is the correct one */ if (saved != mes_key) { saved_cell = ent_get_cell(vehicle_id); saved = ent_get_mes_key(vehicle_id); index = 0; tracked_set_current(tracked, saved_cell, saved); } while (saved != mes_key || saved_cell != mes_key_cell) { int32 next_mes_key_cell; CTDB_MES_NODE_KEY next_mes_key; /* If we have exhausted our list of choices break and * keep the enclosure that ctdb_place_vehicle has placed * us in. */ if (!tracked->node_look_fcn(tracked,index++,&next_mes_key_cell, &next_mes_key)) break; /* If the next choice for a mes_key is the one we had been * placed in then keep the placement and stop looking. */ if ((next_mes_key == mes_key) && (next_mes_key_cell == mes_key_cell)) break; saved_cell = mes_key_cell = next_mes_key_cell; saved = mes_key = next_mes_key; position[Z] = saved_z; ctdb_place_vehicle_ml(gcs_get_tdb(mes_key_cell, FALSE), position[X], position[Y], pdb->dimensions[Y], pdb->dimensions[X], pdb->mass_kg, tracked->desired_dir[X], tracked->desired_dir[Y], &position[Z], hull_to_world, &new_soil, &mes_key); } position[Z] += pdb->model_base_adjustment; /* Store tangential angle for enhanced CCTT mobility model */ if (tracked->params->mobility_model == TRK_CCTT_MOBILITY_MODEL_X) tracked->ccttx_sin_tangent_angle = hull_to_world[X][Z]; } /* save our position to that we can see if it changed later.. (perhaps from a teleport of a vehicle. */ VMAT4_VEC_COPY64(position, tracked->saved_position); /* If the soil changed, look up the new soil */ new_soil = CTDB_SIMNET_SOIL(new_soil); if (new_soil != tracked->params->soils[tracked->soil_latch].soil_type) tracked->soil_latch = tracked_find_soil(new_soil, tracked); /* * Set the entity parameters */ velocity[X] = hull_to_world[Y][X] * speed; velocity[Y] = hull_to_world[Y][Y] * speed; velocity[Z] = hull_to_world[Y][Z] * speed; /* If appropriate, * add effect of wave action on hull. Modifies position and hull_to_world */ velocity[CELL3D] = position[CELL3D]; add_wave_influence(vehicle_id, tracked, dt, cell, position, velocity, hull_to_world, mes_key); ent_set_position_gcs(vehicle_id, position); DEBUG_TRACKED(("TRACKED position: (%.3lf, %.3lf, %.3lf, cell %d) (mes_id: %d encl_id: %d)\n", position[X], position[Y], position[Z], (int32)position[CELL3D], CTDB_MES_NODE_KEY_TO_MES_ID(mes_key), CTDB_MES_NODE_KEY_TO_ENCLOSURE_ID(mes_key))); ent_set_mes_key(vehicle_id, mes_key); ent_set_velocity_gcs(vehicle_id, velocity); ent_set_rotation_gcs(vehicle_id, hull_to_world, (int32)position[CELL3D]); /* save our position to that we can see if it changed later.. (perhaps from a teleport of a vehicle. */ tracked->saved_position[X] = position[X]; tracked->saved_position[Y] = position[Y]; tracked->saved_position[Z] = position[Z]; old_appear = ent_get_appearance(vehicle_id); if ((old_appear & vehDustCloudMask) != dust) ent_set_appearance(vehicle_id, (old_appear & ~vehDustCloudMask) | dust); /* Did we just run out of gas? */ if (scmp_decrement(vehicle_id, munition_Fuel, 0, dt * tracked_fuel_usage_rate(tracked->params, fabs(speed))) == 0.0) { tracked->state = TRACKED_STATE_NOGAS; cmpnt_available(vehicle_id, SM_TrackedHull, FALSE); } /* Reset the control inputs to zero, so we do not just wander off if * we stop getting commands for some period of time. */ #define NO_COMMAND_TIME 2000 if ((time_last_simulation_clock - tracked->last_command_time) > NO_COMMAND_TIME) { float64 dir[2]; tracked->control_state = TRACKED_EXPLICIT; vmat2e_unit64(hull_to_world[Y][X], hull_to_world[Y][Y], dir); tracked->desired_dir[X] = dir[X]; tracked->desired_dir[Y] = dir[Y]; tracked->desired_dir[CELL3D] = cell; tracked->speed = 0.0; } if (trkStats) { float64 newDir[XYZC]; ent_get_direction_gcs(vehicle_id, newDir); hulls_statsTickEnd(trkStats, vehicle_id, tracked->desired_gear, tracked->speed, tracked->desired_dir, position, velocity, newDir, dt); } } static void flat_2D( float64 x, float64 y, float64 *xout, float64 *yout) { float64 mag; mag = x * x + y * y; /* If mag is nearly one, then the vector is already normal */ if ((mag > 0.9999) && (mag < 1.0001)) { *xout = x; *yout = y; } else if (mag == 0.0) { *xout = 1.0; *yout = 0.0; } else { mag = 1.0 / fsqrt(mag); *xout = x * mag; *yout = y * mag; } } /* Add effect of wave action on hull. Modifies position and hull_to_world * returns zero if no effect. */ static int32 add_wave_influence( int32 vehicle_id, TRACKED_VARS *tracked, float64 dt, int32 cell, float64 position[XYZ], float64 velocity[XYZ], float64 hull_to_world[XYZ][XYZ], CTDB_MES_NODE_KEY mes_key) { float64 lposition[XYZC]; float64 lvelocity[XYZ]; float64 lhull_to_world[XYZ][XYZ]; float64 wrot[3][3]; float64 htow[3][3]; int32 soil, new_soil; float64 temp_z; PHYSDB_DATA *pdb; float32 roll, pitch, yaw; ObjectType my_guise; CTDB *ctdb; ENV_INTERFACE ei; /* environmental interface */ CTDB_ELEV_DATA maxelev; static CTDB_ELEV_QUALIFIER qual=(CTDB_INCL_WATER | CTDB_RETURN_MAX | CTDB_INCL_MATERIAL); static int32 test_sign = -1; if (!tracked_wave_effect) return 0; my_guise = ent_get_guise(vehicle_id, 0); if (!tracked_have_rao(my_guise)) return 0; if (!position) { ent_get_position_gcscs(vehicle_id, lposition, cell); ent_get_velocity_gcscs(vehicle_id, lvelocity, cell); ent_get_rotation_gcscs(vehicle_id, lhull_to_world, cell); } else { VMAT3_VEC_COPY64(position, lposition); VMAT3_VEC_COPY64(velocity, lvelocity); VMAT3_MAT_COPY64(hull_to_world, lhull_to_world); } lposition[CELL3D] = cell; /* * We don't want wave effects if we're driving on solid ground. * * The following call asks CTDB for the elevation of the water * surface, if there is one at the given location. */ ctdb = ent_get_ctdb(vehicle_id); soil = ctdb_lookup_soil(ctdb, lposition[X], lposition[Y]); if( CTDB_SIMNET_SOIL(soil) != 4 && CTDB_SIMNET_SOIL(soil) != 5 && CTDB_SIMNET_SOIL(soil) != 9) return 0; vmat3_rotation64_to_zdown_angles32(lhull_to_world, &yaw, &pitch, &roll); lposition[Z] = 0.; pitch = 0.; roll = 0.; /* tracked_run_rao() sets absolute pitch, roll, and position[Z] */ tracked_run_rao(vehicle_id, tracked, my_guise, dt, &roll, &pitch, lposition, &lhull_to_world[Y][0], lvelocity); /* * Calculate the effect of current on our position. The main tick * code has calculated movement across the surface. Here, we add * the effect of movement of that surface. This is analogous to * headwind/tailwind on aircraft. Note that this is distinct from * movement due to wave (surge/heave/sway). */ if (!first_tick) /* dt provided on first tick is bogus */ { bzero(&ei, sizeof(ENV_INTERFACE)); ei.u.sea_current_velocity.input_vector[X] = lposition[X]; ei.u.sea_current_velocity.input_vector[Y] = lposition[Y]; ei.u.sea_current_velocity.input_vector[Z] = lposition[Z]; ei.u.sea_current_velocity.input_vector[CELL3D] = cell; env_get(ENV_SEA_CURRENT_VELOCITY, &ei); lposition[X] += ei.u.sea_current_velocity.output_vector[X] * dt; lposition[Y] += ei.u.sea_current_velocity.output_vector[Y] * dt; } vmat3_zdown_angles32_to_rotation64(yaw, pitch, roll, lhull_to_world); if (!position) { lposition[CELL3D] = cell; ent_set_position_gcs(vehicle_id, lposition); ent_set_rotation_gcs(vehicle_id, lhull_to_world, cell); return 1; } VMAT3_VEC_COPY64(lposition, position); if (hull_to_world) VMAT3_MAT_COPY64(lhull_to_world, hull_to_world); return 1; } static void skew(int32 vehicle_id) { float64 skew[3][3]; float64 htow[3][3]; float64 angle; int32 cell; ent_get_rotation_gcs(vehicle_id, htow, &cell); /* -5 .. -3, 3 .. 5 degrees around Y */ angle = rnd_uniform_dist() * 4.0; if (angle < 2.0) angle -= 5.0; else angle += 1.0; angle = DEG_TO_RAD(angle); vmat3_primary_rotation64(Y, sin(angle), cos(angle), skew); vmat3_mat_mat_mul64(htow, skew, htow); /* -8 .. -6, 6 .. 8 degrees around X */ angle = rnd_uniform_dist() * 4.0; if (angle < 2.0) angle -= 8.0; else angle += 6.0; angle = DEG_TO_RAD(angle); vmat3_primary_rotation64(X, sin(angle), cos(angle), skew); vmat3_mat_mat_mul64(htow, skew, htow); ent_set_rotation_gcs(vehicle_id, htow, cell); } static void get_soil( int32 vehicle_id, TRACKED_VARS *tracked, CTDB *ctdb) { uint32 soil; double position[XYZC]; float64 adj_z; ent_get_position_gcs(vehicle_id, position); adj_z = position[Z] + 1.0;/* safety margin for multi-level */ soil = CTDB_SIMNET_SOIL(ctdb_lookup_soil_ml(ctdb, position[X], position[Y], adj_z)); tracked->soil_latch = tracked_find_soil(soil, tracked); } int32 tracked_find_soil( uint32 soil, TRACKED_VARS *tracked) { int32 i; int32 zero=0; /* Find this soil type (or a zero soil type, by default) */ /* $$$ This is a temporary change to allow an interim solution for * databases with bathymetry to work. Basically, we're redefining * soil type 9. We now want it to behave like soil type 4. We are * putting the change here instead of adding soil type 9 to all of * the vehicle parameter files. */ if(soil == 9) soil = 4; for (i=0;iparams->n_soils;i++) { if (tracked->params->soils[i].soil_type == soil) return i; if (!tracked->params->soils[i].soil_type) zero = i; } return zero; } static void tick_mini_driver( int32 vehicle_id, TRACKED_VARS *tracked, float64 dt) { float64 position[XYZC]; float64 direction[XYZC]; float64 dx, dy; ent_get_position_gcs(vehicle_id, position); ent_get_direction_gcs(vehicle_id, direction); /* make sure that tracked->position is with respect to the * vehicles current cell! Convert it in place... */ if (position[CELL3D] != tracked->position[CELL3D]) { gcs_gcs3_to_gcscs64((int32)tracked->position[CELL3D], tracked->position, (int32)position[CELL3D], position); tracked->position[CELL3D] = position[CELL3D]; } /* * These are stubs. Be sure to replace with better algorithms. */ tracked->max_accel = tracked->max_turn = 0.0; switch (tracked->control_state) { case TRACKED_POSITION_DIRECTION: dx = tracked->position[X] - position[X]; dy = tracked->position[Y] - position[Y]; if (NEARLY_ZERO(dx,1.0) && NEARLY_ZERO(dy,1.0)) { tracked->speed = 0.0; tracked->desired_dir[X] = tracked->direction[X]; tracked->desired_dir[Y] = tracked->direction[Y]; tracked->desired_dir[CELL3D] = tracked->direction[CELL3D]; } else { tracked->desired_dir[CELL3D] = position[CELL3D]; /* Face the point */ if (dx*direction[X] + dy*direction[Y] > 0) { tracked->desired_gear = HULLS_GEAR_FORWARD; tracked->desired_dir[X] = dx; tracked->desired_dir[Y] = dy; } else { tracked->desired_gear = HULLS_GEAR_REVERSE; tracked->desired_dir[X] = -dx; tracked->desired_dir[Y] = -dy; } /* Approach at a speed such that we get halfway to the point * on our next tick. (There is a much better algorithm * which uses acceleration/deceleration information, but * this is just a stub.) */ tracked->speed = 0.5 * fsqrt(dx*dx + dy*dy) / dt; } break; case TRACKED_GOAL_CORRIDOR: /* This stub is worse that the previous one. It just ignores the * corridor altogether. */ tracked->desired_dir[X] = tracked->position[X] - position[X]; tracked->desired_dir[Y] = tracked->position[Y] - position[Y]; tracked->desired_dir[CELL3D] = position[CELL3D]; tracked->desired_gear = HULLS_GEAR_FORWARD; tracked->speed = tracked->approach_speed; break; case TRACKED_TARGET_ID: /* Where is this guy? */ ent_get_position_gcscs(tracked->target_id, tracked->position, (int32)position[CELL3D]); tracked->position[CELL3D] = position[CELL3D]; case TRACKED_TARGET_POSITION: /* Ramming speed! */ tracked->desired_dir[X] = tracked->position[X] - position[X]; tracked->desired_dir[Y] = tracked->position[Y] - position[Y]; tracked->desired_dir[CELL3D] = position[CELL3D]; tracked->desired_gear = HULLS_GEAR_FORWARD; tracked->speed = 1000.0; /* A really big number meaning MAXIMUM */ break; } } float64 tracked_compute_eta( int32 vehicle_id, TRACKED_VARS *tracked, float64 *target, int32 cell) { struct tracked_soils *soil; float64 position[XYZ]; float64 dx, dy, dist; ent_get_position_gcscs(vehicle_id, position, cell); /* Make sure we know what soil type we're using (only happens * before first tick). */ if (tracked->soil_latch == -1) soil = tracked->params->soils; /* Use 0 by default */ else soil = tracked->params->soils+tracked->soil_latch; /* This algorithm has much room for improvement: * -> consider acceleration to reach max speed * -> consider soil types in between * -> consider grade in between */ dx = target[X] - position[X]; dy = target[Y] - position[Y]; dist = fsqrt(dx*dx + dy*dy); return (dist / soil->max_speeds_mps[TRACKED_MAX_FORWARD]); } /* ARGSUSED */ void tracked_collision( int32 vehicle_id, COLLISION_NOTIFICATION_DATA *data, ADDRESS user_data) { TRACKED_VARS *tracked = (TRACKED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), tracked_user_data_handle); float64 position[XYZC]; int32 other_id; int32 coll_type; float64 my_pos[XYZC]; float64 velocity[XYZC]; float64 dp[XYZ]; float64 other_velocity[XYZ]; float64 other_mass; float64 speed; float64 d_momentum; PHYSDB_DATA *pdb; int32 cell = ent_get_cell(vehicle_id); if (!tracked) /* Passive error detection */ return; /* Copy some useful data out of the collision data */ coll_type = data->coll_type; other_id = data->other_id; other_mass = data->other_mass; /* this is equivelent to position := data->position except that * it insures everything is with repect to the entities current * cell */ gcs_gcs3_to_gcscs64(data->cell, data->position, cell, position); position[CELL3D] = cell; /* ditto for velocity.. */ gcs_vec_to_vec64(data->cell, data->other_velocity, cell, other_velocity); /* Note: * * Rather than doing a real Newtonian model (which would be pretty * easy) to figure out our new velocity, we instead do this stuff. * This is because other SIMNET simulators don't use a real model, * so if we decided to just plow forward, we would probably penetrate * the other party in the collision. That would be bad. */ /* If we aren't moving, allow ourselves to be nudged */ if (ent_get_stationary(vehicle_id)) { pdb = ent_get_physdb(vehicle_id); /* Assume the collision is perfectly inelastic, so our * combined masses will move at some new velocity such that: * momentum == (m1 + m2) * new_velocity * so, new_velocity = (momentum) / (m1 + m2) */ /* Compute his momentum */ d_momentum = other_mass / (other_mass + pdb->mass_kg); VMAT3_SCAL_VEC_MUL(d_momentum, other_velocity, velocity); /* Rather than doing all the friction calculations to figure * out how long we would slide at this rate, just apply a simple * heuristic. */ VMAT3_SCAL_VEC_MUL(SLIDE_TIME, velocity, dp); ent_get_position_gcs(vehicle_id, my_pos); VMAT3_VEC_ADD(my_pos, dp, my_pos); ent_set_position_gcs(vehicle_id, my_pos); /* save our position to that we can see if it changed later.. (perhaps from a teleport of a vehicle. */ VMAT4_VEC_COPY64(position, tracked->saved_position); } else { ent_get_velocity_gcs(vehicle_id, velocity); /* NOTE: This is broken under SIMNET. If the other guy rear ends * us, and detects the collision first, he will immediately stop * moving. When we then get the collision packet, it looks like * he isn't moving so we assume we collided with him. The net * result is that we both recoil from the impact. */ /* Ignore the collision unless we and the other party are moving * in opposite directions or we were moving faster (implying * that *we* rear-ended him). */ if ((VMAT2_DOT_PROD(velocity, other_velocity) <= 0.0) || (ent_get_speed_squared(vehicle_id) > VMAT3_DOT_PROD(other_velocity, other_velocity))) { /* Recoil at 1 mps (this shouldn't look to bad, and will * give the driver a moment to react). */ speed = -1.0 / ent_get_speed(vehicle_id); VMAT3_SCAL_VEC_MUL(speed, velocity, velocity); ent_set_velocity_gcs(vehicle_id, velocity); } } } void tracked_bridge_collision( int32 vehicle_id, int32 bridge_id, ADDRESS user_data) { float64 bridge_pos[XYZC], my_pos[XYZC]; float64 dist1, dist2, dx, dy; TRACKED_VARS *tracked = (TRACKED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), tracked_user_data_handle); if (!tracked) /* Passive error detection */ return; /* Check to see if this bridge is closer than the current `bridge_id'. */ if (tracked->bridge_id && (tracked->bridge_id != bridge_id)) { ent_get_position_gcs(vehicle_id, my_pos); ent_get_position_gcscs(bridge_id, bridge_pos, (int32)my_pos[CELL3D]); dx = bridge_pos[X] - my_pos[X]; dy = bridge_pos[Y] - my_pos[Y]; dist1 = dx*dx + dy*dy; ent_get_position_gcscs(tracked->bridge_id, bridge_pos, (int32)my_pos[CELL3D]); dx = bridge_pos[X] - my_pos[X]; dy = bridge_pos[Y] - my_pos[Y]; dist2 = dx*dx + dy*dy; if(dist1 > dist2) return; } tracked->bridge_id = bridge_id; tracked->bridge_collision_timeout = time_last_simulation_clock + 5000; } static void tracked_request_smoke( int32 vehicle_id) { float64 velocity[XYZC]; ENV_EFFECT_INTERFACE ei; ent_get_velocity_gcs(vehicle_id, velocity); if ((velocity[X] != 0.0) || (velocity[Y] != 0.0) || (velocity[Z] != 0.0)) { sched_deferred_fncl(tracked_request_smoke, 500, vehicle_id, A_INT, vehicle_id, A_END); return; } /* Request the smoke type for this vehicle */ #if 0 ei.u.smoke.source = ent_get_guise(vehicle_id, 0); #else ei.u.smoke.source = environmental_Smoke_DFR_Medium; #endif ent_get_position_gcs(vehicle_id, ei.u.smoke.location); ei.u.smoke.num_sources = 1; ei.u.smoke.source_type = ENV_SOURCE_POINT_DETONATION; env_request_effect(ENV_SMOKE_EFFECT, &ei); } void tracked_damage( int32 vehicle_id, uint32 new_kill_level, uint32 old_kill_level, ADDRESS user_data) { char msg[100]; int32 damaged; int32 is_cat_kill; uint32 mask = ENT_CAT_KILL_MASK | ENT_MOB_KILL_MASK; TRACKED_VARS *tracked = (TRACKED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), tracked_user_data_handle); if (!tracked || !tracked->cells_initted) /* Passive error detection */ return; /* if this isn't a transition in the cat or mob bits return */ if(!((new_kill_level ^ old_kill_level) & mask)) return; /* Set the damage field bassed on if the fields were turned */ /* on (damage is true) or turned off (damage is false). */ /* Both cat and fire must be off for a repair. */ if(new_kill_level & mask) damaged = TRUE; else damaged = FALSE; /* Create smoke if this is a new cat_kill */ if((new_kill_level & ENT_CAT_KILL_MASK) && !(old_kill_level & ENT_CAT_KILL_MASK)) tracked_request_smoke(vehicle_id); /* If this is a cat kill set the is_cat_kill flag */ if((new_kill_level & ENT_CAT_KILL_MASK) || (old_kill_level & ENT_CAT_KILL_MASK)) is_cat_kill = TRUE; else is_cat_kill = FALSE; if (damaged) ent_set_appearance_bits(vehicle_id, vehMobilityDisabled); else ent_unset_appearance_bits(vehicle_id, vehMobilityDisabled); if (is_cat_kill) { if (damaged) { /* tracked_request_smoke(vehicle_id); */ ent_set_appearance_bits(vehicle_id, (vehDestroyed | vehSmokePlume | vehFlaming)); sched_simtime_fncl(ent_unset_appearance_bits, time_last_simulation_clock + (15 * 60 * 1000), vehicle_id, A_INT, vehicle_id, A_INT, vehFlaming, A_END); sched_simtime_fncl(ent_unset_appearance_bits, time_last_simulation_clock + (30 * 60 * 1000), vehicle_id, A_INT, vehicle_id, A_INT, vehSmokePlume, A_END); if (!((old_kill_level & new_kill_level) & ENT_CAT_KILL_MASK)) { /* Notify the Operator this is a catastrophic kill */ sprintf(msg, "Vehicle %d catastrophic kill", vehicle_id); alert_user(vehicle_id, 0, ALERT_DEST_VEH_IIGUI, msg); /* When a vehicle is destroyed a secondary explosion occurs */ arty_send_detonation_PDU( munition_US_Mk82, munition_US_M904, (int32)tracked->saved_position[CELL3D], tracked->saved_position, 0); } } else ent_unset_appearance_bits(vehicle_id, vehDestroyed | vehSmokePlume | vehFlaming); } if (damaged) { /* Nothing to do if the vehicle is already dead or dying */ if ((tracked->state == TRACKED_STATE_DEAD) || (tracked->state == TRACKED_STATE_DYING)) return; else tracked->state = TRACKED_STATE_DYING; } else { if (tracked->state == TRACKED_STATE_DEAD) { tracked->state = TRACKED_STATE_HEALTHY; cmpnt_available(vehicle_id, SM_TrackedHull, TRUE); } } } /* * cctt_get_index() * * cctt_get_index computes a discreet value from a continous range, * given the value, minimum, maximum, and level of discretization. * The maximum value must be greater than the minimum. * */ static int32 cctt_get_index( float64 value, float64 min, float64 max, int32 n_values) /* input value */ /* minimum value */ /* maximum value */ /* number of values */ { int32 index; /* discritization */ /* error checking */ if (min == max) return 0; /* offset value */ value -= min; /* round to nearest integer */ index = (int32) ((float64) n_values)* ((float64) value)/((float64) (max-min)); /* check index */ if (index < 0) return 0; if (index >= n_values) return n_values-1; return index; } static int32 trk_wet_weather_on( TRACKED_VARS *tracked, float64 position[XYZC]) { CTDB_ABSTRACT_DATA *data; ENV_INTERFACE ei; bzero(&ei, sizeof(ENV_INTERFACE)); ei.u.generic_visual_check.input_viewer[X] = position[X]; ei.u.generic_visual_check.input_viewer[Y] = position[Y]; ei.u.generic_visual_check.input_viewer[Z] = position[Z]; ei.u.generic_visual_check.input_viewer[CELL3D] = position[CELL3D]; if(time_last_simulation_clock - tracked->last_check_time > CHECK_PERIOD) { tracked->last_check_time = time_last_simulation_clock; tracked->within_hydro_subsurface = ctdb_point_within_abstract((CTDB*)gcs_get_tdb((int32)position[CELL3D], FALSE), CTDB_ABSTRACT_HYDRO_SUBSURFACE, position[X], position[Y], &data) && data->hydro_subsurface.wetness; } if ((env_get(ENV_PRECIPITATION_RATE, &ei) != -1) && (env_get(ENV_PRECIPITATION_TYPE, &ei) != -1)) if (((ei.u.precipitation_rate.output_value > 0.0) && (ei.u.precipitation_type.output_type > 0)) || tracked->within_hydro_subsurface) return(TRUE); else return(FALSE); return(FALSE); } static int32 trk_in_trouble_slope( float64 soil_slope, TRACKED_VARS *tracked, float64 position[XYZC], struct tracked_soils *soil) { if(trk_wet_weather_on(tracked, position) && (soil_slope > soil->max_wet_slope)) return TRUE; if(!trk_wet_weather_on(tracked, position) && (soil_slope > soil->max_dry_slope)) return TRUE; return FALSE; } static int32 trk_simnet_to_cctt_soil_mapping( TRACKED_VARS *tracked, float64 position[XYZC], int32 soil_type) { int32 i; if(trk_wet_weather_on(tracked, position)) /* wet weather */ { for (i=0; i< TRACKED_MAX_SOIL_TYPE; i++) { if (tracked_soil_tables[1].soil_table[i].simnet_soil_type == soil_type) return(tracked_soil_tables[1].soil_table[i].cctt_soil_type); } return(0); } else { for (i=0; i< TRACKED_MAX_SOIL_TYPE; i++) { if (tracked_soil_tables[0].soil_table[i].simnet_soil_type == soil_type) return(tracked_soil_tables[0].soil_table[i].cctt_soil_type); } return(0); } return(0); } /*======================================================================= turn on statistics gathering, called from parser */ void tracked_statsOn(void) { if (!trkStats) hulls_statsInit(&trkStats); else hulls_statsOn(trkStats); tracked_statsStatus(); } /*======================================================================== turn off statistics gathering, called from parser */ void tracked_statsOff(void) { if (trkStats) hulls_statsOff(trkStats); tracked_statsStatus(); } /*======================================================================== zero out statistics, called from parser */ void tracked_statsReset(void) { if (trkStats) { hulls_statsReset(trkStats); tracked_statsStatus(); } else printf("No tracked stats memory allocated, use \"On\" command\n"); } /*======================================================================== print out the current statistics, called from parser */ void tracked_statsPrint(void) { if (trkStats) { printf("Tracked stats:\n"); hulls_statsPrint(trkStats); } else printf("No tracked stats data, use \"on\" command\n"); } /*======================================================================== printout the current settings for statistics gathering */ void tracked_statsStatus(void) { printf("Tracked stats "); hulls_statsStatus(trkStats); return; } /*======================================================================= delete statistics and deallocate memory */ void tracked_statsQuit(void) { if (trkStats) hulls_statsDestroy(&trkStats); tracked_statsStatus(); }