/**************************************************************************** * File: whl_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: wtaylor * * $Revision$ * * Remarks: * * * ****************************************************************************/ #include #include "libwhl_local.h" #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include /*common/include/global*/ #include #include #define WHL_LOWER_TICK_BOUND 0.08 #define WHL_UPPER_TICK_BOUND 0.12 #define WHL_MIN_RES_LEVEL 0 #define WHL_MAX_RES_LEVEL 2 #define WHL_HALF_PI 1.570796327 #define WHL_HALF_PI_INVERSE 0.6366197724 #define WHL_TO_DEG 57.29577951 #define WHL_G_CONST 9.81 #define NEARLY_ZERO(x,err) (((x) > -err) && ((x) < err)) #define CHECK_PERIOD 10000 /* miliseconds */ #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 */ #define PI_4 0.7853990675 /* PRIVATE FUNCTIONS ------------------------------------------------------------*/ static int32 whl_in_trouble_slope( float64 soil_slope, WHEELED_VARS *wheeled, float64 position[XYZC], struct wheeled_soils *soil); static int32 whl_wet_weather_on( WHEELED_VARS *wheeled, float64 position[XYZC]); static int32 whl_simnet_to_cctt_soil_mapping( WHEELED_VARS *wheeled, float64 position[XYZC], int32 soil_type); static float64 whl_get_veh_max_speed( WHEELED_VARS *wheeled, float64 position[XYZC], int32 cctt_soil); static void skew( int32 vehicle_id); static void get_soil( WHEELED_VARS *wheeled, float64 position[XYZC]); static void flat_2D( float64 x, float64 y, float64 *xout, float64 *yout); static void mini_driver( WHEELED_VARS *wheeled, float64 desired_speed, float64 speed, float64 delta_t, float64 *desired_direction, float64 *direction, int32 *current_gear, float64 *desired_wheel_angle, float64 *desired_acceleration, float64 *desired_brake, int32 *desired_gear); #if 0 static void tick_mini_driver( int32 vehicle_id, WHEELED_VARS *wheeled, float64 dt); #endif static void dynamics( WHEELED_VARS *wheeled, PHYSDB_DATA *pdb, float64 max_speed, int32 soil_type, float64 desired_speed, float64 desired_wheel_angle, float64 desired_acceleration, float64 desired_brake, int32 desired_gear, float64 delta_t, float64 *position, float64 *turn_angle, float64 *speed, int32 *current_gear); static int32 cctt_get_index( float64 value, /* input value */ float64 min, /* minimum value */ float64 max, /* maximum value */ int32 n_values); /* number of values */ /* hook for wheeled movement statistics */ static HULLS_STATS *wheeledStats = NULL; /* WHEELED TICK * * First computes resolution level, then gets vehicle inputs from a * mini driver and then ticks the dynamics according to that * resolution under those inputs. */ void wheeled_tick( int32 vehicle_id, uint32 interval_since_last_tick, ADDRESS user_data) { WHEELED_VARS *wheeled = (WHEELED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), wheeled_user_data_handle); float64 position[XYZC]; float64 velocity[XYZC]; float64 direction[XYZC]; float64 hull_to_world[XYZ][XYZ]; float64 bridge_dir[XYZ], bridge_pos[XYZ]; float64 temp_vec[XY]; float64 dx, dy, odx, ody; /* orientation of the vehicle */ int32 still, right_direction; float64 speed, desired_speed, desired_acceleration, desired_direction[2], desired_wheel_angle, desired_brake; int32 desired_gear, current_gear; struct wheeled_soils *soil; CTDB_MES_NODE_KEY mes_key; int32 mes_key_cell; int32 new_soil; float64 max_mps, dz; float64 turn_angle=0.0; float64 dt; float64 cross, dot; float64 slope; float64 dist_from_center; PHYSDB_DATA *pdb, *bridge_pdb; uint32 old_appear, dust; int32 soil_type; CTDB *ctdb = ent_get_ctdb(vehicle_id); int32 num_lanes, i; float64 dist, temp_dist; float64 slope_sin, soil_slope, veh_max_speed; int32 cctt_soil; int32 cell = ent_get_cell(vehicle_id); if (!wheeled) /* Passive error detection */ return; if (wheeled->being_moved) /* We're being towed, don't bother */ return; /* Nothing to do if the vehicle is dead. Still may want to compute dynamics, though, if, for instance, someone else collides with dead vehicle. */ if (wheeled->state == WHEELED_STATE_DEAD) { /* may want to check for a collision here. how to do that? */ return; } dt = interval_since_last_tick / 1000.0; /* Careful about stopping the simulation */ if (dt == 0.0) return; /* 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(wheeledStats) hulls_statsTickStart(wheeledStats, vehicle_id, wheeled->speed, position, direction, dt); /* get previous turn angle */ turn_angle = NS_ATAN2(direction[Y],direction[X]); /* get previous speed speed = sqrt(velocity[X]*velocity[X] + velocity[Y]*velocity[Y] + velocity[Z]*velocity[Z]);*/ /* Cache whether we are not moving */ still = ((velocity[X] == 0.0) && (velocity[Y] == 0.0)); /* make sure that wheeled->desired_dir is with respect to * the entities current cell. */ if (cell != wheeled->desired_dir[CELL3D]) { /* perform the conversion in place.. */ gcs_vec_to_vec64((int32)wheeled->desired_dir[CELL3D], wheeled->desired_dir, (int32)cell, wheeled->desired_dir); wheeled->desired_dir[CELL3D] = cell; } if (!wheeled->cells_initted) { /* this is a hack. since we cannot find out the vehicles * current direction in wheeled_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.. */ wheeled->cells_initted = TRUE; VMAT4_VEC_COPY64(direction, wheeled->desired_dir); wheeled->node_keys.list_cell[0] = ent_get_cell(vehicle_id); } if (!wheeled->node_look_fcn(wheeled,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], wheeled->saved_position[X]))) || (!(CLOSE_ENOUGH(position[Y], wheeled->saved_position[Y]))) || (position[CELL3D] != wheeled->saved_position[CELL3D])) { pdb = ent_get_physdb(vehicle_id); ctdb_place_vehicle_ml(ctdb, position[X], position[Y], pdb->dimensions[Y], pdb->dimensions[X], pdb->mass_kg, wheeled->desired_dir[X], wheeled->desired_dir[Y], position+Z, hull_to_world, &new_soil, &mes_key); ent_set_rotation_gcs(vehicle_id, hull_to_world, cell); /* save our position to that we can see if it changed later.. * (perhaps from a teleport of a vehicle. */ VMAT4_VEC_COPY64(position, wheeled->saved_position); /* 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); wheeled_clear_node_key_list(wheeled,mes_key_cell,mes_key); wheeled_set_current(wheeled,mes_key_cell,mes_key); } /* 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]*wheeled->desired_dir[Y] - direction[Y]*wheeled->desired_dir[X]; dot = direction[X]*wheeled->desired_dir[X] + direction[Y]*wheeled->desired_dir[Y]; right_direction = NEARLY_ZERO(cross,0.001) && (dot > 0.0); /* Grab the desired speed. */ desired_speed = wheeled->speed; /* Are we out of gas? */ if (wheeled->state == WHEELED_STATE_NOGAS) { /* Has some kind soul given us more? */ if (scmp_get_amount(vehicle_id, munition_Fuel, 0) != 0.0) { wheeled->state = WHEELED_STATE_HEALTHY; cmpnt_available(vehicle_id, SM_WheeledHull, TRUE); } else { /* If we have no gas, we keep ticking dynamics just * until we stop. */ if (still) return; desired_speed = 0.0; /* this is not right */ } } /* Are we getting stuck? */ if (wheeled->state == WHEELED_STATE_STICKING) { /* Just like running out of gas. Wait till we stop moving. */ if (still) { wheeled->state = WHEELED_STATE_STUCK; wheeled->stuck_position[X] = position[X]; wheeled->stuck_position[Y] = position[Y]; wheeled->stuck_position[CELL3D] = position[CELL3D]; cmpnt_available(vehicle_id, SM_WheeledHull, FALSE); return; } } /* Are we stuck? */ if (wheeled->state == WHEELED_STATE_STUCK) { /* check to make sure everything is in the same cell. */ if (cell != wheeled->stuck_position[CELL3D]) { /* convert it in place. */ gcs_gcs2_to_gcscs64((int32)wheeled->stuck_position[CELL3D], wheeled->stuck_position, cell, wheeled->stuck_position); wheeled->stuck_position[CELL3D] = cell; } /* Have we been towed? */ dx = position[X] - wheeled->stuck_position[X]; dy = position[Y] - wheeled->stuck_position[Y]; if (!NEARLY_ZERO(dx,0.001) || !NEARLY_ZERO(dy,0.001)) { wheeled->state = WHEELED_STATE_HEALTHY; cmpnt_available(vehicle_id, SM_WheeledHull, TRUE); /* Note that we don't know what we're on now. */ wheeled->soil_latch = -1; } else return; } if (wheeled->state == WHEELED_STATE_DYING) { /* Just like running out of gas. Wait till we stop moving. */ if (still) { if (wheeled_mkill_cants_vehicle) skew(vehicle_id); wheeled->state = WHEELED_STATE_DEAD; cmpnt_available(vehicle_id, SM_WheeledHull, 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 && (wheeled->soil_latch != -1)) return; pdb = ent_get_physdb(vehicle_id); /* Figure out if we are on a bridge or not. */ if(wheeled->bridge_id) { float64 bridge_poly[4][2]; float64 bridge_parallel[2], bridge_perp[2]; ent_get_position_gcscs(wheeled->bridge_id, bridge_pos, (int32)position[CELL3D]); ent_get_direction_gcscs(wheeled->bridge_id, bridge_dir, (int32)position[CELL3D]); /* Compute the polygon which surrounds the bridge. */ bridge_pdb = ent_get_physdb(wheeled->bridge_id); 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, bridge_parallel, bridge_parallel); VMAT2_SCAL_VEC_MUL(bridge_pdb->dimensions[X] * 0.5, 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? */ wheeled->on_bridge = geo_is_point_in_convex_polygon(position, bridge_poly, 4) == GEO_PT_INSIDE; if(wheeled->on_bridge) { READER_UNION *speeds = reader_find_tag(reader_get_symbol("on_bridge"), wheeled_speeds, READER_UNTAGGED, READER_NO_ERRORS); wheeled->soil_latch = 0; desired_speed = NS_MIN(desired_speed, speeds[2].real); } if(!wheeled->on_bridge && time_last_simulation_clock > wheeled->bridge_collision_timeout) { wheeled->bridge_id = 0; wheeled->soil_latch = -1; } } /* How fast are we going right now? */ speed = ent_get_speed(vehicle_id); #if 0 /* Do we want to change gear? */ if (wheeled->current_gear != wheeled->desired_gear) { if (speed < 0.1) wheeled->current_gear = wheeled->desired_gear; else /* Don't allow gear change until we stop */ desired_speed = 0.0; } #endif /* Make sure we know what soil type we're using */ if (wheeled->soil_latch == -1) get_soil(wheeled, position); /* Get the parameters for the soil we're on. */ soil = wheeled->params->soils+wheeled->soil_latch; soil_type = soil->soil_type; /* See if we can't move through this stuff. */ if (soil->max_speeds_mps[WHEELED_MAX_FORWARD] == 0.0) { wheeled->state = WHEELED_STATE_STICKING; DEBUG_WHEELED(("Vehicle %d: About to get stuck in soil type %d\n", vehicle_id, soil->soil_type)); } else { /* See if we coasted through */ if (wheeled->state == WHEELED_STATE_STICKING) wheeled->state = WHEELED_STATE_HEALTHY; } /* Get slope */ slope_sin = direction[Z]; soil_slope = asin(fabs(slope_sin)); soil_slope = RAD_TO_DEG(soil_slope); if (wheeled_mobility || wheeled_mobility_plus_allow_env) { cctt_soil = whl_simnet_to_cctt_soil_mapping(wheeled, position, soil->soil_type); if (cctt_soil < 0) cctt_soil = 0; veh_max_speed = whl_get_veh_max_speed(wheeled, position, cctt_soil); if (desired_speed > veh_max_speed) desired_speed = veh_max_speed; } if ((wheeled_mobility || wheeled_mobility_plus_allow_env) && (wheeled->state != WHEELED_STATE_START_TROUBLE) && (wheeled->state != WHEELED_STATE_IN_TROUBLE)) { if(whl_in_trouble_slope(soil_slope, wheeled, position, soil)) { wheeled->state = WHEELED_STATE_START_TROUBLE; wheeled->waiting_in_trouble_state = time_last_simulation_clock; wheeled->saved_desired_speed = desired_speed; } } if (wheeled->state == WHEELED_STATE_START_TROUBLE) { if ((wheeled->waiting_in_trouble_state + 7000) > time_last_simulation_clock) { desired_speed = 0.0; } else { wheeled->state = WHEELED_STATE_IN_TROUBLE; } } if(wheeled->state == WHEELED_STATE_IN_TROUBLE) { if (whl_in_trouble_slope(soil_slope, wheeled, position, soil)) { if ((wheeled->waiting_in_trouble_state + wheeled->params->in_trouble_limit) < (time_last_simulation_clock)) { desired_speed = wheeled->saved_desired_speed; wheeled->state = WHEELED_STATE_STICKING; } } else wheeled->state = WHEELED_STATE_HEALTHY; } /* * ModSAF Vehicle Dynmaics Section * * This section computes the ModSAF Wheeled Mobility Model dynamics. * * Input: * wheeled->desired_dir - desired direction * desired_speed - desired speed * * Output: * dx, dy - direction * position[] - position */ if (wheeled->params->mobility_model == WHL_MODSAF_MOBILITY_MODEL) { /* How fast can we go on this soil? */ if (wheeled->current_gear == HULLS_GEAR_FORWARD) { max_mps = soil->max_speeds_mps[WHEELED_MAX_FORWARD]; dz = direction[Z]; } else { max_mps = soil->max_speeds_mps[WHEELED_MAX_REVERSE]; dz = -direction[Z]; } /* Adjust for grade */ if(!wheeled->on_bridge) if (dz > soil->max_climb_sin) /* Too steep */ max_mps = 0.0; /* this isn't right */ else if (dz > 0.0) /* Not downhill */ max_mps *= 1.0 - sqrt(dz / soil->max_climb_sin); /* Magic */ /* Make our desired direction a 2D unit vector */ flat_2D(wheeled->desired_dir[X], wheeled->desired_dir[Y], &wheeled->desired_dir[X], &wheeled->desired_dir[Y]); desired_direction[X] = wheeled->desired_dir[X]; desired_direction[Y] = wheeled->desired_dir[Y]; /* get the pointer to the physical parameters structure for this vehicle */ pdb = ent_get_physdb(vehicle_id); /* Calculate the level of dynamics resolution. The algorithm for * doing this is evolutionary. Right now (1/17/95) it is very * simple: a simple function of tick time located within a band. * Outside the band the resolution level changes, within the band * it doesn't. There is some dynamics to this, indicative of * future algorithms. They should all have some good dynamics, * but not too complicated that they defeat the purpose of having * multiple resolutions, which is to speed the program. */ mini_driver(wheeled, desired_speed, speed, dt, desired_direction, direction, ¤t_gear, &desired_wheel_angle, &desired_acceleration, &desired_brake, &desired_gear); /* Now that we have the desired inputs to the vehicle, let's * calculate the dynamics of the vehicle. Note that the only * inputs to the dynamics function should be the only inputs that * you could give to the actual vehicle. In this case these are * desired wheel angle, desired acceleration, deisred braking, and * desired gear. However, we could put in desired seating * position, lights, wipers, stereo volume, whatever. The * important thing is that they are inputs to an actual vehicle. * The ONE exception (so far) is the desired complexity of the * dynamics. This should be put in as an argument in terms of * programming only, NOT in terms of vehicle dynamics. So, the * complexity level will be passed, but only with great * reluctance, since it makes things easier. Of course, some * vehicle parameters must be passed as well. Also, the states of * the vehicle must be passed (to be changed within the function). * For now these states are speed, turn angle, and x and y * position increments. */ dynamics(wheeled, pdb, max_mps, soil_type, desired_speed, desired_wheel_angle, desired_acceleration, desired_brake, desired_gear, dt, position, &turn_angle, &speed, ¤t_gear); /* compute direction components */ dx = wheeled->direction[X] = cos(turn_angle); dy = wheeled->direction[Y] = sin(turn_angle); } /* * 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: * wheeled->dx,dy - desired direction * desired_speed - desired speed * * Output: * dx, dy - direction * speed - speed * */ else if (wheeled->params->mobility_model == WHL_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 */ float64 cctt_rot[2][2]; /* rotation matrix */ 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 */ /* physical parameters */ pdb = ent_get_physdb(vehicle_id); /* 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) wheeled_cctt_get_table_value( &wheeled_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 - wheeled->last_check_time > CHECK_PERIOD) { wheeled->last_check_time = time_last_simulation_clock; wheeled->within_hydro_surface = ctdb_point_within_abstract(ctdb, CTDB_ABSTRACT_HYDRO_SURFACE, position[X], position[Y], &data) && data->hydro_surface.wetness; wheeled->within_hydro_subsurface = ctdb_point_within_abstract(ctdb, CTDB_ABSTRACT_HYDRO_SUBSURFACE, position[X], position[Y], &data) && data->hydro_subsurface.wetness; } if(wheeled->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 && !wheeled->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 || wheeled->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) wheeled_cctt_get_table_value(&wheeled_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 = wheeled_cctt_get_table_value( &wheeled_cctt_table_max_speed, cctt_index); /* Determine Local Gear */ cctt_local_gear = wheeled->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(wheeled->desired_dir[X], wheeled->desired_dir[Y], &wheeled->desired_dir[X], &wheeled->desired_dir[Y]); flat_2D(direction[X], direction[Y], &dx, &dy); /* Determine Heading Error */ cctt_heading_error = vmat2e_angle_between_vectors64(dx, dy, wheeled->desired_dir[X], wheeled->desired_dir[Y]); /* Determine Heading Error Rate */ cctt_heading_error_rate = wheeled->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 = wheeled_cctt_get_table_value( &wheeled_cctt_table_max_turn_rate, cctt_index); /* Limit Heading Error Rate */ if (cctt_heading_error_rate > soil->max_turn_rps) cctt_heading_error_rate = soil->max_turn_rps; else if (cctt_heading_error_rate < -soil->max_turn_rps) cctt_heading_error_rate = -soil->max_turn_rps; /* NONHOLONOMIC CONSTRAINT: Limit turn angle based on minimum turn radius */ if (cctt_heading_error_rate > wheeled->params->turn_radius_inv*speed) cctt_heading_error_rate = wheeled->params->turn_radius_inv*speed; else if (cctt_heading_error_rate < -wheeled->params->turn_radius_inv*speed) cctt_heading_error_rate = -wheeled->params->turn_radius_inv*speed; /* Determine Turn Angle */ turn_angle = cctt_heading_error_rate * dt; /* Compute New Angle */ vmat2_primary_rotation64(sin(turn_angle), cos(turn_angle), cctt_rot); odx = dx, ody = dy; dx = odx * cctt_rot[X][X] + ody * cctt_rot[Y][X]; dy = odx * cctt_rot[X][Y] + ody * cctt_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 (soil->max_speeds_mps[WHEELED_MAX_FORWARD] > 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; wheeled->commanded_throttle_pos = 0.0; } /* Apply Throttle */ else { /* compute modified acceleration value */ cctt_accel_mod = wheeled->acceleration; /* compute commanded throttle position rate */ cctt_commanded_throttle_pos_rate = wheeled->params->throttle_forward_loop_gain* (SGN(cctt_speed_error)*cctt_speed_error_limited_abs - wheeled->params->throttle_accel_feedback_gain*cctt_accel_mod); /* integrate throttle position */ wheeled->commanded_throttle_pos += cctt_commanded_throttle_pos_rate*dt; /* limit commanded throttle position */ if (wheeled->commanded_throttle_pos > 1.0) wheeled->commanded_throttle_pos = 1.0; else if (wheeled->commanded_throttle_pos < 0.0) wheeled->commanded_throttle_pos = 0.0; cctt_brake_pos_percent = 0.0; } /* compute throttle position */ if (cctt_local_commanded_speed > 0.0) wheeled->throttle_pos += wheeled->params->tau_throttle_inv* dt*(wheeled->commanded_throttle_pos - wheeled->throttle_pos); else wheeled->throttle_pos = 0.0; /* limit throttle position */ if (wheeled->throttle_pos > 1.0) wheeled->throttle_pos = 1.0; else if (wheeled->throttle_pos < 0.0) wheeled->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(wheeled->throttle_pos, 0.0, 1.0, wheeled->params->n_throttle_positions); cctt_index[3] = cctt_get_index(cctt_slope, 0.0, PI_4, wheeled->params->n_slopes); cctt_index[4] = cctt_get_index(speed, 0.0, cctt_max_speed, wheeled->params->n_speeds); cctt_force_drive = wheeled_cctt_get_table_value(&wheeled_cctt_table_force_drive, cctt_index); /* Scale drive force for reverse direction */ if (cctt_local_gear == WHEELED_CCTT_GEAR_REVERSE) cctt_force_drive *= wheeled->params->reverse_drive_factor; /* Determine Braking Force */ if ((speed > WHEELED_CCTT_THRESHOLD) && (cctt_brake_pos_percent > WHEELED_CCTT_THRESHOLD_BRAKE)) { cctt_index[0] = cctt_vehicle_class; cctt_index[1] = cctt_soil_type; cctt_brake_linear_factor = wheeled_cctt_get_table_value( &wheeled_cctt_table_force_brake, cctt_index); cctt_force_brake = WHEELED_CCTT_GRAVITY*pdb->mass_kg * cctt_brake_pos_percent * cctt_brake_linear_factor; } else cctt_force_brake = 0.0; /* Determine Gravitational Force */ cctt_weight_longitudinal = WHEELED_CCTT_GRAVITY*pdb->mass_kg*cctt_slope_sin; cctt_weight_vertical = WHEELED_CCTT_GRAVITY*pdb->mass_kg*cctt_slope_cos; /* Ignore Gravity Going Downhill */ if (((cctt_slope < 0.0) && (cctt_local_gear == WHEELED_CCTT_GEAR_FORWARD)) || ((cctt_slope > 0.0) && (cctt_local_gear == WHEELED_CCTT_GEAR_REVERSE))) { cctt_weight_longitudinal = 0.0; cctt_weight_vertical = pdb->mass_kg*WHEELED_CCTT_GRAVITY; } /* Determine Friction Force */ cctt_index[0] = cctt_vehicle_class; cctt_index[1] = cctt_soil_type; cctt_mu_eff_coeff = wheeled_cctt_get_table_value( &wheeled_cctt_table_friction, cctt_index); cctt_force_friction = cctt_mu_eff_coeff*cctt_weight_vertical; if (NEARLY_ZERO(speed, WHEELED_CCTT_THRESHOLD)) { cctt_force_friction = 0.0; cctt_weight_longitudinal = 0.0; } /* Sum Forces */ if (cctt_local_gear == WHEELED_CCTT_GEAR_FORWARD) { cctt_force = cctt_force_drive - cctt_weight_longitudinal - cctt_force_friction - cctt_force_brake; } else if (cctt_local_gear == WHEELED_CCTT_GEAR_REVERSE) { cctt_force = -cctt_force_drive - cctt_weight_longitudinal + cctt_force_friction + cctt_force_brake; } else cctt_force = 0.0; /* Compute Acceleration */ wheeled->acceleration = cctt_force/pdb->mass_kg; /* Compute Speed */ if (cctt_local_gear == WHEELED_CCTT_GEAR_FORWARD) { speed += wheeled->acceleration*dt; } else { speed -= wheeled->acceleration*dt; } speed = fabs(speed); if (NEARLY_ZERO(speed, WHEELED_CCTT_THRESHOLD)) speed = 0.0; else if (speed > cctt_max_speed) speed = cctt_max_speed; if (wheeled->current_gear == HULLS_GEAR_REVERSE) { speed = -speed; } /* Integrate position (this is included in the ModSAF dynamics subroutine)*/ position[X] += speed * direction[X] * dt; position[Y] += speed * direction[Y] * dt; } /* 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; #if 0 /* Don't do anything expensive if we're already pointing the * right way. */ if (right_direction) { dx = wheeled->desired_dir[X]; dy = wheeled->desired_dir[Y]; /* Update our position for the distance we moved since * last tick. */ if (!still) { position[X] += velocity[X] * dt; position[Y] += velocity[Y] * dt; } } else { /* Get our current direction in a 2D unit vector */ flat_2D(direction[X], direction[Y], &dx, &dy); } #endif /* If the vehicle is on the bridge, get its position and rotation * matrix from the bridge instead of the ground. */ if(wheeled->on_bridge) { float64 x_axis[XYZ], y_axis[XYZ], theta; wheeled->bridge_collision_timeout = time_last_simulation_clock; ent_get_rotation_gcscs(wheeled->bridge_id, hull_to_world, (int32)position[CELL3D]); /* 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) && wheeled->node_look_fcn(wheeled,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, dx, dy, &position[Z], hull_to_world, &new_soil, &mes_key); if (saved != mes_key) { saved_cell = ent_get_cell(vehicle_id); saved = ent_get_mes_key(vehicle_id); index = 0; wheeled_set_current(wheeled, saved_cell, saved); } /* If this was not the correct enclosure start looking ahead in * the list to see if the next enclosure (...) is the correct one */ 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 (!wheeled->node_look_fcn(wheeled,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, dx, dy, &position[Z], hull_to_world, &new_soil, &mes_key); } position[Z] += pdb->model_base_adjustment; } wheeled->g_component = hull_to_world[Y][Z]; /* save our position so that we can see if it changed later.. * (perhaps from a teleport of a vehicle. */ VMAT4_VEC_COPY64(position, wheeled->saved_position); new_soil = CTDB_SIMNET_SOIL(new_soil); /* If the soil changed, look up the new soil */ if (new_soil != wheeled->params->soils[wheeled->soil_latch].soil_type) wheeled->soil_latch = wheeled_find_soil(new_soil, wheeled); /* * Set the entity parameters.. note that velocity[CELL3D] has already * been set by the ent_get_velocity_function... */ if (wheeled->current_gear == HULLS_GEAR_FORWARD) { velocity[X] = hull_to_world[Y][X] * speed; velocity[Y] = hull_to_world[Y][Y] * speed; velocity[Z] = hull_to_world[Y][Z] * speed; } else { velocity[X] = -hull_to_world[Y][X] * speed; velocity[Y] = -hull_to_world[Y][Y] * speed; velocity[Z] = -hull_to_world[Y][Z] * speed; dust = 0; } ent_set_position_gcs(vehicle_id, position); DEBUG_WHEELED(("WHEELED position: (%.3lf, %.3lf, %.3lf) (mes_id: %d encl_id: %d)\n", position[X], position[Y], position[Z], 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]); 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 * wheeled_fuel_usage_rate(wheeled->params, speed)) == 0.0) { wheeled->state = WHEELED_STATE_NOGAS; cmpnt_available(vehicle_id, SM_WheeledHull, FALSE); } /* Reset the control inputs to zero at the end of each tick, so we * do not just wander off if we stop getting commands. */ #define NO_COMMAND_TIME 2000 if ((time_last_simulation_clock - wheeled->last_command_time) > NO_COMMAND_TIME) { float64 dir[2]; wheeled->control_state = WHEELED_EXPLICIT; vmat2e_unit64(hull_to_world[Y][X], hull_to_world[Y][Y], dir); wheeled->desired_dir[X] = dir[X]; wheeled->desired_dir[Y] = dir[Y]; wheeled->desired_dir[CELL3D] = cell; wheeled->speed = 0.0; } if (wheeledStats) { float64 newDir[XYZC]; ent_get_direction_gcs(vehicle_id, newDir); hulls_statsTickEnd(wheeledStats, vehicle_id, wheeled->desired_gear, wheeled->speed, wheeled->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 = 0.0; *yout = 0.0; } else { mag = 1.0 / fsqrt(mag); *xout = x * mag; *yout = y * mag; } } static void skew( int32 vehicle_id) { float64 skew[XYZ][XYZ]; float64 htow[XYZ][XYZ]; 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( WHEELED_VARS *wheeled, float64 position[XYZC]) { uint32 soil; float64 adj_z = position[Z] + 1.0; /* safety margin for multi-level */ CTDB *ctdb = (CTDB*)gcs_get_tdb(position[CELL3D], FALSE); soil = CTDB_SIMNET_SOIL(ctdb_lookup_soil_ml(ctdb, position[X], position[Y], adj_z)); wheeled->soil_latch = wheeled_find_soil(soil, wheeled); } int32 wheeled_find_soil( uint32 soil, WHEELED_VARS *wheeled) { 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 (wheeled->params->soils[i].soil_type == soil) return i; if (!wheeled->params->soils[i].soil_type) zero = i; } return zero; } #if 0 static void tick_mini_driver( int32 vehicle_id, WHEELED_VARS *wheeled, 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 wheeled->position is with respect to the * vehicles current cell! Convert it in place... */ if (position[CELL3D] != wheeled->position[CELL3D]) { gcs_gcs3_to_gcscs64((int32)wheeled->position[CELL3D], wheeled->position, (int32)position[CELL3D], position); wheeled->position[CELL3D] = position[CELL3D]; } /* * These are stubs. Be sure to replace with better algorithms. */ wheeled->max_accel = wheeled->max_turn = 0.0; switch (wheeled->control_state) { case WHEELED_POSITION_DIRECTION: dx = wheeled->position[X] - position[X]; dy = wheeled->position[Y] - position[Y]; if (NEARLY_ZERO(dx,1.0) && NEARLY_ZERO(dy,1.0)) { wheeled->speed = 0.0; wheeled->desired_dir[X] = wheeled->direction[X]; wheeled->desired_dir[Y] = wheeled->direction[Y]; wheeled->desired_dir[CELL3D] = wheeled->direction[CELL3D]; } else { wheeled->desired_dir[CELL3D] = position[CELL3D]; /* Face the point */ if (dx*direction[X] + dy*direction[Y] > 0) { wheeled->desired_gear = HULLS_GEAR_FORWARD; wheeled->desired_dir[X] = dx; wheeled->desired_dir[Y] = dy; } else { wheeled->desired_gear = HULLS_GEAR_REVERSE; wheeled->desired_dir[X] = -dx; wheeled->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.) */ wheeled->speed = 0.5 * fsqrt(dx*dx + dy*dy) / dt; } break; case WHEELED_GOAL_CORRIDOR: /* This stub is worse that the previous one. It just ignores the * corridor altogether. */ wheeled->desired_dir[X] = wheeled->position[X] - position[X]; wheeled->desired_dir[Y] = wheeled->position[Y] - position[Y]; wheeled->desired_dir[CELL3D] = position[CELL3D]; wheeled->desired_gear = HULLS_GEAR_FORWARD; wheeled->speed = wheeled->approach_speed; break; case WHEELED_TARGET_ID: /* Where is this guy? */ ent_get_position_gcscs(wheeled->target_id, wheeled->position, (int32)position[CELL3D]); wheeled->position[CELL3D] = position[CELL3D]; case WHEELED_TARGET_POSITION: /* Ramming speed! */ wheeled->desired_dir[X] = wheeled->position[X] - position[X]; wheeled->desired_dir[Y] = wheeled->position[Y] - position[Y]; wheeled->desired_dir[CELL3D] = position[CELL3D]; wheeled->desired_gear = HULLS_GEAR_FORWARD; wheeled->speed = 1000.0; /* A really big number meaning MAXIMUM */ break; } } #endif float64 wheeled_compute_eta( int32 vehicle_id, WHEELED_VARS *wheeled, float64 *target, int32 cell) { struct wheeled_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 (wheeled->soil_latch == -1) soil = wheeled->params->soils; /* Use 0 by default */ else soil = wheeled->params->soils+wheeled->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[WHEELED_MAX_FORWARD]); } /* ARGSUSED */ void wheeled_collision( int32 vehicle_id, COLLISION_NOTIFICATION_DATA *data, ADDRESS user_data) { WHEELED_VARS *wheeled = (WHEELED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), wheeled_user_data_handle); float64 position[XYZC]; 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 (!wheeled) /* Passive error detection */ return; 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, wheeled->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 wheeled_bridge_collision( int32 vehicle_id, int32 bridge_id, ADDRESS user_data) { float64 bridge_pos[XYZ], my_pos[XYZC]; float64 dist1, dist2, dx, dy; WHEELED_VARS *wheeled = (WHEELED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), wheeled_user_data_handle); if (!wheeled) /* Passive error detection */ return; /* Check to see if this bridge is closer than the current `bridge_id'. */ if(wheeled->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(wheeled->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; } wheeled->bridge_id = bridge_id; wheeled->bridge_collision_timeout = time_last_simulation_clock + 5000; } static void wheeled_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(wheeled_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 wheeled_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; WHEELED_VARS *wheeled = (WHEELED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), wheeled_user_data_handle); if (!wheeled) /* 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; /* 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; /* Create smoke if this is a new cat_kill */ if((new_kill_level & ENT_CAT_KILL_MASK) && !(old_kill_level & ENT_CAT_KILL_MASK)) wheeled_request_smoke(vehicle_id); if (damaged) ent_set_appearance_bits(vehicle_id, vehMobilityDisabled); else ent_unset_appearance_bits(vehicle_id, vehMobilityDisabled); if (is_cat_kill) { if (damaged) { 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)wheeled->saved_position[CELL3D], wheeled->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 ((wheeled->state == WHEELED_STATE_DEAD) || (wheeled->state == WHEELED_STATE_DYING)) return; else wheeled->state = WHEELED_STATE_DYING; } else { if (wheeled->state == WHEELED_STATE_DEAD) { wheeled->state = WHEELED_STATE_HEALTHY; cmpnt_available(vehicle_id, SM_WheeledHull, TRUE); } } } #if 0 /* not used and not GCS aware */ /* GET_RES_LEVEL * * $$$ Note: this function is never being called, and it has a * inherit problem with rel_level not being initialized to an * appropriate value. * * get_res_level computes the resolution level to be used in the * *current tick of dynamics. It uses a neutral bacn of tick times. * */ static int32 get_res_level( float64 delta_t, WHEELED_VARS *wheeled) { /* declare variables */ int32 tick_location, res_adder; int32 res_level = 0; /* $$$ This does not make sense!! */ /* check where tick time is: below, in, or above neutral band */ if (delta_t < WHL_LOWER_TICK_BOUND) tick_location = -1; /* below */ else if ((delta_t >= WHL_LOWER_TICK_BOUND) && (delta_t <= WHL_UPPER_TICK_BOUND)) tick_location = 0; /* within */ else tick_location = 1; /* above */ /* start a counter if the level is out of the band */ if (tick_location == 1) wheeled->res_tick_count++; else if (tick_location == 0) wheeled->res_tick_count = 0; else if (tick_location == -1) wheeled->res_tick_count--; else wheeled->res_tick_count = wheeled->res_tick_count; /* if counter is at a certain value, increment or decrement, as appropriate, the resolution, using a incrementer: res_adder */ if (wheeled->res_tick_count >= 10) { res_adder = -1; wheeled->res_tick_count = 0; } else if (wheeled->res_tick_count <= -10) { res_adder = 1; wheeled->res_tick_count = 0; } else res_adder = 0; /* increment (or decrement, etc.) the res level until it hits its extremes */ if ((res_level += res_adder) < WHL_MIN_RES_LEVEL) res_level = WHL_MIN_RES_LEVEL; else if (res_level > WHL_MAX_RES_LEVEL) res_level = WHL_MAX_RES_LEVEL; return(res_level); } #endif /* not used and not GCS aware */ /* MINI DRIVER * * mini_driver computes desired acceleration and desired wheel angle * based on the input desired speed and direction. */ static void mini_driver( WHEELED_VARS *wheeled, float64 desired_speed, float64 speed, float64 delta_t, float64 *desired_direction, float64 *direction, int32 *current_gear, float64 *desired_wheel_angle, float64 *desired_acceleration, float64 *desired_brake, int32 *desired_gear) { /* change internal representation of gear position to my own (which makes more sense). */ if (wheeled->desired_gear == HULLS_GEAR_REVERSE) *desired_gear = -1; else if (wheeled->desired_gear == HULLS_GEAR_FORWARD) *desired_gear = 1; else if (wheeled->desired_gear == 0) *desired_gear = 0; else *desired_gear = 2; if (wheeled->current_gear == HULLS_GEAR_REVERSE) *current_gear = -1; else if (wheeled->current_gear == HULLS_GEAR_FORWARD) *current_gear = 1; else if (wheeled->current_gear == 0) *current_gear = 0; else *current_gear = 2; /* compute desired wheel angle from angle between desired and * actual directions. From this wheel angle, we may also wish to * determine if we desire reverse. As when the desired wheel * angle is larger than 90 degrees. In these cases it may be * simpler to move the vehicle backwards, turning the wheel in the * opposite direction. Of course, then we have the problem of how * long a drive is acceptable in reverse. We don't want to back * up 200 miles just because our desired direction is directly * behind us. Hmm. */ *desired_wheel_angle = vmat2e_angle_between_vectors64(direction[X], direction[Y], wheeled->desired_dir[X], wheeled->desired_dir[Y]); if (fabs(*desired_wheel_angle) < .001) *desired_wheel_angle = 10.0 * *desired_wheel_angle; if (fabs(*desired_wheel_angle) >= 3.14) { *desired_gear = -1; desired_speed = 1.5; /* This does not do anything to outside */ *desired_wheel_angle = 0.0; } else if (fabs(*desired_wheel_angle) > WHL_HALF_PI) { *desired_gear = -1; *desired_wheel_angle = (float64)SGN(*desired_wheel_angle) * (PI - fabs(*desired_wheel_angle)); } /* { *desired_wheel_angle = 10.0 * *desired_wheel_angle; } */ /* check if desired gear must be changed from forward to reverse * or vice versa, in which case we'd like the driver to stop * before he changes the gears. This is sort of cheating because * we want the driver to decide on his own that he needs to stop, * but for now it will do. */ if (SGN(*desired_gear) != SGN(*current_gear)) desired_speed = 0.0; /* Then compute the desired acceleration based on a proportional * control algorithm modified by the tick time. Note that this * acceleration is not direction dependent. That is, negative * acceleration implies braking and positive acceleration implies * increasing engine speed (whether in a forward or reverse * gear). */ *desired_acceleration = (desired_speed-fabs(speed)) / delta_t; /* Here we calculate the desired brake pressure. This one's going * to be tough to make realistic, since even the stupidest, most * non-intuitive drivers vary how they brake in an intuitive and * pretty complex way (according to weather and surface * conditions, etc.). I am currently (as of 1/13/95) looking for * ideas on how to compute braking pressure. For now, I will * leave it at a constant when the desired acceleration is less * than zero. The braking pressure will be quantified in terms of * percent of maximum (maximum what? pedal travel). */ if (*desired_acceleration < 0.0) *desired_brake = 50; else *desired_brake = 0.0; } /* DYNAMICS * * dynamics computes the next tick of dynamics. It outputs new * position, orientation, and speed, as well as wheel angle (for use * as well as wheel angle (for use in next tick). It computes these * based on the resolution level computed in get_res_level. * * everything is assumed to be with respect to the same cell! */ static void dynamics( WHEELED_VARS *wheeled, PHYSDB_DATA *pdb, float64 max_speed, int32 soil_type, float64 desired_speed, float64 desired_wheel_angle, float64 desired_acceleration, float64 desired_brake, int32 desired_gear, float64 delta_t, float64 *position, float64 *turn_angle, float64 *speed, int32 *current_gear) { /* Declare local variables */ float64 max_accel; float64 accel; float64 wheel_angle; float64 tan_inverse; float64 delta_radial, delta_tangential, delta_speed, delta_turn_angle; float64 IC_radius, radius_direction; float64 g_force, wheel_force, brake_force; float64 torque_to_wheel = 0.0; float64 wheel_radius = 0.0; float64 num_drive_wheels = 0.0; wheeled->resolution_level = 1; /* assign a nominal acceleration and deceleration to the vehicle * for use on the lowest resolution */ wheeled->nominal_acceleration = 3.0; wheeled->nominal_deceleration = -30.0; /* First thing to do is to check if the vehicle is in the desired * gear. If the gears must change from a forward gear to reverse * or vice versa then the vehicle must stop. From forward to * another forward, no stopping is necessary. Later on I may add * in engine speed as a function of gear so that idiotic drivers * can blow up their engines. Actually, what really must happen * is the gear box must stop spinning, right. I mean you could be * slipping on ice and stop the wheels and put it into reverse * even if you're still moving forward. We'll have to see. Heck, * maybe I'll even let drivers grind their gears if that's not too * unreasonable, and they have manual transmission. */ if(desired_gear != *current_gear) { if ((SGN(desired_gear) != SGN(*current_gear)) && (fabs(*speed) > .001)) *current_gear = *current_gear; else *current_gear = desired_gear; } /* Compute actual acceleration based on desired acceleration and * gravitational load. The gravitational load portion is * caclulated from the hull_to_world matrix. I need to know the * weight of the vehicle for this calculation. Also, I need to * figure out how I am going to incorporate it into the * acceleration calculation. For now I am just going to use it to * modify the maximum acceleration, which is not too unreasonable, * since for now I am assuming that the "driver" is smart enough * to compensate (using the gas pedal) for any deviations in the * acceleration from that desired (see mini_driver above). I * think I am going to make this a swappable block (2nd level). * Later, I'll incorporate it into a gas pedal model where the * driver must compensate for hills explicitly. (that'll be * another level) */ max_accel = wheeled->params->soils->max_accel_mps2; if (wheeled->resolution_level == 0) { if (desired_acceleration > 0.01) accel = wheeled->nominal_acceleration; else if (desired_acceleration < -0.01) accel = wheeled->nominal_deceleration; else accel = 0.0; } if (wheeled->resolution_level == 1) { if (max_accel != 0.0) max_accel = max_accel - WHL_G_CONST * wheeled->g_component; /* if we get a negative max_accel, vehicle drift uphill. this is bad */ if (max_accel < 0) max_accel = 0; if ((max_accel != 0.0) && (desired_acceleration > max_accel)) accel = max_accel; else if ((max_accel != 0.0) && (desired_acceleration < -30.0*max_accel)) accel = -30.0*max_accel; else accel = desired_acceleration; } /* $$$ NOTE: wheeled->resolution_level is always set to 1 at the * top of this routine, so this code will never be executed. It * will not work properly if it is executed because wheel radius, * torque_to_wheel and num_drive_wheels are not initialized correctly. */ if (wheeled->resolution_level == 2) { if (wheel_radius < .01) { fprintf(stderr,"Wheel radius is WAY too small"); exit(-1); } if (pdb->mass_kg < 50.0) { fprintf(stderr,"Vehicle mass is WAY too small"); exit(-1); } /* First, calculate the net internally supplied force to the * terrain from the wheels. This includes engine power and * brakes, since, in actuality, they can be applied at the * same time (and are, in fact, on some traction control * systems). */ wheel_force = (torque_to_wheel / wheel_radius) * num_drive_wheels; brake_force = desired_brake * SGN(*current_gear) * SGN(*speed); /* Now, sum these to the gravity force and divide the sum by the * mass of the vehicle to get the acceleration on "sticky" terrain. */ g_force = WHL_G_CONST * (float64) pdb->mass_kg * wheeled->g_component; accel = (wheel_force + brake_force + g_force) / pdb->mass_kg; /* Now, we can "estimate" what size force the terrain can * support. That is, if it is largely hard terrain, we need * the friction component on the wheels. Once this is * exceeded the wheels begin to slip and a new acceleration * will be computed. Once the wheels are slipping we then * need to determine when they "catch" again as well as a new * set of dynamics. */ } /* check if vehicle length is too small, since we are dividing by it */ if (pdb->dimensions[Y] < .0001) { fprintf(stderr,"Vehicle length is WAY too small"); exit(-1); } /* Compute actual steering angle based on steering linkage limits */ if (fabs(desired_wheel_angle) > MAX_WHEEL_ANGLE) wheel_angle = MAX_WHEEL_ANGLE * (float64) SGN(desired_wheel_angle); else wheel_angle = desired_wheel_angle; /*--------------------------------------------------*/ /*--------------------------------------------------*/ /* compute kinematics. */ /* The following are lowest resolution kinematics */ if (wheeled->resolution_level == 0) { /* position changes according to current direction and speed, * no steering angle input is used */ position[X] += *speed * cos(*turn_angle) * delta_t; position[Y] += *speed * sin(*turn_angle) * delta_t; /* turn angle changes according to magnitude of wheel angle and * speed */ delta_turn_angle = (*speed) * delta_t * wheel_angle * WHL_HALF_PI_INVERSE / (0.5 * pdb->dimensions[Y]); } /* The following are no-slip kinematics. Automatic if either * resolution level=1 or if resolution level is >1 and the terrain * is of asphalt type (soil_type=1) (very "sticky"). */ if ((wheeled->resolution_level == 1) || (soil_type == 1 && wheeled->resolution_level > 1)) { /* check if wheel angle is zero (straight) so we don't divide by zero */ if (fabs(wheel_angle) <= 0.000001) { /* only position changes, not rotation */ position[X] += *speed * cos(*turn_angle) * delta_t * SGN(*current_gear); position[Y] += *speed * sin(*turn_angle) * delta_t * SGN(*current_gear); delta_turn_angle = 0.0; } /* check if the wheel angle is 90 degrees so we can deal with * infinity. In actuality, this should not happen, since the * if-else block before this one disallows wheel angles * greater than some specified value (which should be less * than 90 degrees), but just in case, you know, if a vehicle * is developed that can turn its wheels all the way around or * something. */ else if (fabs(wheel_angle) >= 1.570795327) { /* only rotation angle changes, not position */ delta_turn_angle = (*speed / pdb->dimensions[Y]) * delta_t * (float64) SGN(wheel_angle); } else { /* magnitude of instantaneous center of rotation */ tan_inverse = 1.0 / tan(wheel_angle); /*printf("tan_inv: %.2f ",tan_inverse);*/ IC_radius = pdb->dimensions[Y] * sqrt(tan_inverse*tan_inverse + 0.25); /* direction of radius from vehicle centroid to IC */ radius_direction = *turn_angle + (wheel_angle/2.0) + 1.570796327; /* change in orientation of angle of vehicle */ delta_turn_angle = (*speed / IC_radius) * delta_t * (float64) SGN(wheel_angle) * SGN(*current_gear); /* change in position in vehicle coordinates */ delta_radial = IC_radius * (1.0 - cos(delta_turn_angle)); delta_tangential = (SGN(delta_turn_angle) * IC_radius * sin(delta_turn_angle)) * SGN(*current_gear); /* change in position in world coordinates */ position[X] += delta_radial * cos(radius_direction) + delta_tangential * sin(radius_direction); position[Y] += delta_radial * sin(radius_direction) + delta_tangential * cos(radius_direction) * -1.0; } } else if ((wheeled->resolution_level == 2) && (soil_type >2)) { if (soil_type == 3) /* soft sandy soil type */ { ; } } /* compute new speed */ if (wheeled->resolution_level != 0) { delta_speed = accel * delta_t; *speed += delta_speed; } else *speed = desired_speed; if (fabs(*speed) > max_speed) *speed = fabs(max_speed) * (float64) SGN(*speed); /* compute new turn angle */ *turn_angle = *turn_angle + delta_turn_angle; /* Convert gear designations back to internal representation */ if (*current_gear == -1) { wheeled->current_gear = HULLS_GEAR_REVERSE; wheeled->node_look_fcn = (WHEELED_NODE_KEY_LOOK_FCN)wheeled_node_key_look_back; } else if (*current_gear == 0) wheeled->current_gear = 0; else { wheeled->current_gear = HULLS_GEAR_FORWARD; wheeled->node_look_fcn = (WHEELED_NODE_KEY_LOOK_FCN)wheeled_node_key_look_ahead; } } static int32 whl_wet_weather_on( WHEELED_VARS *wheeled, 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 - wheeled->last_check_time > CHECK_PERIOD) { wheeled->last_check_time = time_last_simulation_clock; wheeled->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)) || wheeled->within_hydro_subsurface) return(TRUE); else return(FALSE); return(FALSE); } static int32 whl_in_trouble_slope(float64 soil_slope, WHEELED_VARS *wheeled, float64 position[XYZC], struct wheeled_soils *soil) { if(whl_wet_weather_on(wheeled, position) && (soil_slope > soil->max_wet_slope)) return TRUE; if(!whl_wet_weather_on(wheeled, position) && (soil_slope > soil->max_dry_slope)) return TRUE; return FALSE; } static int32 whl_simnet_to_cctt_soil_mapping(WHEELED_VARS *wheeled, float64 position[XYZC], int32 soil_type) { int32 i; /* This data is copied from libtracked. Will be updated when actual * data received */ if(whl_wet_weather_on(wheeled, position)) /* wet weather */ { for (i=0; i< WHEELED_MAX_SOIL_TYPE; i++) { if (wheeled_soil_tables[1].soil_table[i].simnet_soil_type == soil_type) return(wheeled_soil_tables[1].soil_table[i].cctt_soil_type); } return(0); } else { for (i=0; i< WHEELED_MAX_SOIL_TYPE; i++) { if (wheeled_soil_tables[0].soil_table[i].simnet_soil_type == soil_type) return(wheeled_soil_tables[0].soil_table[i].cctt_soil_type); } return(0); } return(0); } static float64 whl_get_veh_max_speed(WHEELED_VARS *wheeled, float64 position[XYZC], int32 cctt_soil) { int32 i; /* This data is copied from libtracked. Will be updated when actual * data received */ if(whl_wet_weather_on(wheeled, position)) /* wet weather */ { for (i=0; i< WHEELED_MAX_SOIL_TYPE; i++) { if (wheeled_speed_tables[1].speed_table[i].simnet_soil_type == cctt_soil) return(wheeled_speed_tables[1].speed_table[i].max_speed); } return(20.0); } else { for (i=0; i< WHEELED_MAX_SOIL_TYPE; i++) { if (wheeled_speed_tables[0].speed_table[i].simnet_soil_type == cctt_soil) return(wheeled_speed_tables[0].speed_table[i].max_speed); } return(29.0); } } /* * 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, /* input value */ float64 min, /* minimum value */ float64 max, /* maximum value */ int32 n_values) /* 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; } /*======================================================================= turn on statistics gathering, called from parser */ void wheeled_statsOn(void) { if (!wheeledStats) hulls_statsInit(&wheeledStats); else hulls_statsOn(wheeledStats); wheeled_statsStatus(); } /*======================================================================== turn off statistics gathering, called from parser */ void wheeled_statsOff(void) { if (wheeledStats) hulls_statsOff(wheeledStats); wheeled_statsStatus(); } /*======================================================================== zero out statistics, called from parser */ void wheeled_statsReset(void) { if (wheeledStats) { hulls_statsReset(wheeledStats); wheeled_statsStatus(); } else printf("No wheeled stats memory allocated, use \"On\" command\n"); } /*======================================================================== print out the current statistics, called from parser */ void wheeled_statsPrint(void) { if (wheeledStats) { printf("Wheeled stats:\n"); hulls_statsPrint(wheeledStats); } else printf("No wheeled stats data, use \"on\" command\n"); } /*======================================================================== printout the current settings for statistics gathering */ void wheeled_statsStatus(void) { printf("Wheeled stats "); hulls_statsStatus(wheeledStats); return; } /*======================================================================= delete statistics and deallocate memory */ void wheeled_statsQuit(void) { if (wheeledStats) hulls_statsDestroy(&wheeledStats); wheeled_statsStatus(); }