/**************************************************************************** * File: trk_class.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: Manages class information attached to each vehicle * * Created: Mon Jul 13 11:35:57 PDT 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 /*common/include/global*/ #include /*common/include/global*/ int32 tracked_user_data_handle; static void tracked_print(CLASS_USER_DATA_TYPE vars); static void get_trouble_state( int32 vehicle_id, int32 user_data_handle, struct hulls_get_trouble_state *data); static void tracked_create( int32 vehicle_id, int32 user_data_handle, TRACKED_PARAMETRIC_DATA *params); static void tracked_destroy( int32 vehicle_id, int32 user_data_handle); static void set_dir_speed( int32 vehicle_id, int32 user_data_handle, struct hulls_set_direction_speed *data); static void set_vel_gear( int32 vehicle_id, int32 user_data_handle, struct hulls_set_velocity_gear *data); static void set_vel_dir( int32 vehicle_id, int32 user_data_handle, struct hulls_set_velocity_direction *data); static void set_vel_ori( int32 vehicle_id, int32 user_data_handle, struct hulls_set_velocity_orientation *data); static void set_pos_dir( int32 vehicle_id, int32 user_data_handle, struct hulls_set_position_direction *data); static void set_goal_corr( int32 vehicle_id, int32 user_data_handle, struct hulls_set_goal_corridor *data); static void set_target_id( int32 vehicle_id, int32 user_data_handle, struct hulls_set_target_id *data); static void set_target_position( int32 vehicle_id, int32 user_data_handle, struct hulls_set_target_position *data); static void get_eta( int32 vehicle_id, int32 user_data_handle, struct hulls_get_eta *data); static void set_takeoff( int32 vehicle_id, int32 user_data_handle, struct hulls_set_takeoff *data); static void set_fly_level( int32 vehicle_id, int32 user_data_handle, struct hulls_set_fly_level *data); static void get_max_range( int32 vehicle_id, int32 user_data_handle, struct hulls_get_max_range *data); static void set_external_control( int32 vehicle_id, int32 user_data_handle, struct hulls_set_velocity_direction *data); static void get_limits( int32 vehicle_id, int32 user_data_handle, struct hulls_get_limits *data); static void trk_started_being_moved( int32 vehicle_id, int32 vehicle_moving_me, int32 dynamics, ADDRESS user_data); static void trk_stopped_being_moved( int32 vehicle_id, int32 vehicle_moving_me, float64 *final_pos, ADDRESS user_data); static void set_mes_key( int32 vehicle_id, int32 user_data_handle, struct hulls_set_mes_key *data); static void get_mes_key( int32 vehicle_id, int32 user_data_handle, struct hulls_get_mes_key *data); void tracked_init_subclass(void) { tracked_user_data_handle = class_reserve_user_data(disdbobj_class, "tracked", tracked_print); /* Tell libcomponents we are available. */ cmpnt_define_instance(SM_TrackedHull, 1, &tracked_user_data_handle, (CMPNT_CREATE)tracked_create, (CMPNT_DESTROY)tracked_destroy, HULLS_SET_DIRECTION_SPEED_FCN, set_dir_speed, HULLS_SET_VELOCITY_GEAR_FCN, set_vel_gear, HULLS_SET_VELOCITY_DIRECTION_FCN, set_vel_dir, HULLS_SET_VELOCITY_ORIENTATION_FCN, set_vel_ori, HULLS_SET_POSITION_DIRECTION_FCN, set_pos_dir, HULLS_SET_GOAL_CORRIDOR_FCN, set_goal_corr, HULLS_SET_TARGET_ID_FCN, set_target_id, HULLS_SET_TARGET_POSITION_FCN, set_target_position, HULLS_GET_ETA_FCN, get_eta, HULLS_SET_TAKEOFF_FCN, set_takeoff, HULLS_SET_FLY_LEVEL_FCN,set_fly_level, HULLS_GET_MAX_RANGE_FCN, get_max_range, HULLS_SET_EXTERNAL_CONTROL_FCN, set_external_control, HULLS_GET_LIMITS_FCN, get_limits, HULLS_SET_MES_KEY_FCN, set_mes_key, HULLS_GET_MES_KEY_FCN, get_mes_key, HULLS_GET_TROUBLE_STATE_FCN, get_trouble_state, A_END); /* * Register handlers for collisions. */ callback_register_handler(coll_notify_event, tracked_collision, NULL); callback_register_handler(coll_bridge_notify_event, tracked_bridge_collision, NULL); /* * Register handlers for receiving damage/repairs. */ callback_register_handler(ent_kill_level_notify, tracked_damage, NULL); } static char *state_string( TRACKED_STATE x) { switch(x) { case TRACKED_STATE_HEALTHY: return "TRACKED_STATE_HEALTHY"; case TRACKED_STATE_NOGAS: return "TRACKED_STATE_NOGAS"; case TRACKED_STATE_STICKING: return "TRACKED_STATE_STICKING"; case TRACKED_STATE_STUCK: return "TRACKED_STATE_STUCK"; case TRACKED_STATE_DYING: return "TRACKED_STATE_DYING"; case TRACKED_STATE_DEAD: return "TRACKED_STATE_DEAD"; } return "?"; } static char *control_state_string( TRACKED_CONTROL_STATE x) { switch(x) { case TRACKED_EXPLICIT: return "TRACKED_EXPLICIT"; case TRACKED_POSITION_DIRECTION: return "TRACKED_POSITION_DIRECTION"; case TRACKED_GOAL_CORRIDOR: return "TRACKED_GOAL_CORRIDOR"; case TRACKED_TARGET_ID: return "TRACKED_TARGET_ID"; case TRACKED_TARGET_POSITION: return "TRACKED_TARGET_POSITION"; } return "?"; } #define NODE_PRINT(which,string) (i == tracked->node_keys.which ? string :" ") static void tracked_print_node_key_list( TRACKED_VARS *tracked) { int32 i; printf(" node_key history:\n"); for (i=0;i mes %6d encl %6d cell %d\n",NODE_PRINT(current,"C"), NODE_PRINT(first,"F"), NODE_PRINT(last,"L"), CTDB_MES_NODE_KEY_TO_MES_ID(tracked->node_keys.list[i]), CTDB_MES_NODE_KEY_TO_ENCLOSURE_ID(tracked->node_keys.list[i]), tracked->node_keys.list_cell[i]); if ((TRACKED_NODE_KEY_LOOK_FCN)tracked->node_look_fcn == (TRACKED_NODE_KEY_LOOK_FCN)tracked_node_key_look_ahead ) printf(" Node Key Function in look ahead mode\n"); else printf(" Node Key Function in look back mode\n"); } static void tracked_print( CLASS_USER_DATA_TYPE vars) { TRACKED_VARS *tracked = (TRACKED_VARS *)vars; if (tracked->soil_latch) { printf(" current limits (soil = %d):\n", tracked->params->soils[tracked->soil_latch].soil_type); printf(" max_speeds_mps: forward %f, backward %f\n", tracked->params-> soils[tracked->soil_latch].max_speeds_mps[TRACKED_MAX_FORWARD], tracked->params-> soils[tracked->soil_latch].max_speeds_mps[TRACKED_MAX_REVERSE]); printf(" max_accel_mps2: %f\n", tracked->params->soils[tracked->soil_latch].max_accel_mps2); printf(" max_decel_mps2: %f\n", tracked->params->soils[tracked->soil_latch].max_decel_mps2); printf(" max_turn_rps: %f\n", tracked->params->soils[tracked->soil_latch].max_turn_rps); printf(" sin(max_climb): %f\n", tracked->params->soils[tracked->soil_latch].max_climb_sin); } printf(" state: %s\n", state_string(tracked->state)); printf(" current gear: %s\n", (tracked->current_gear == HULLS_GEAR_FORWARD) ? "forward" : "reverse"); if (tracked->state == TRACKED_STATE_STUCK) printf(" stuck position: <%f %f cell: %d>\n", tracked->stuck_position[X], tracked->stuck_position[Y], (int32)tracked->stuck_position[CELL3D]); printf(" control state: %s\n", control_state_string(tracked->control_state)); switch(tracked->control_state) { case TRACKED_GOAL_CORRIDOR: printf(" approach speed: %f\n", tracked->approach_speed); printf(" corridor width: %f\n", tracked->corridor_width); /* Fall through */ case TRACKED_POSITION_DIRECTION: printf(" desired position: <%f %f cell: %d>\n", tracked->position[X], tracked->position[Y], (int32)tracked->position[CELL3D]); printf(" desired direction: <%f %f cell: %d>\n", tracked->direction[X], tracked->direction[Y], (int32)tracked->direction[CELL3D]); break; case TRACKED_TARGET_ID: printf(" target id: %d\n", tracked->target_id); /* Fall through */ case TRACKED_TARGET_POSITION: printf(" target position: <%f %f cell: %d>\n", tracked->position[X], tracked->position[Y], (int32)tracked->position[CELL3D]); } printf(" desired direction: <%f %f cell: %d>\n", tracked->desired_dir[X], tracked->desired_dir[Y], (int32)tracked->desired_dir[CELL3D]); printf(" desired speed: %f\n", tracked->speed); printf(" desired gear: %s\n", (tracked->desired_gear == HULLS_GEAR_FORWARD) ? "forward" : "reverse"); printf(" max turn rate: %f\n", tracked->max_turn); printf(" max acceleration: %f\n", tracked->max_accel); tracked_print_node_key_list(tracked); } void tracked_set_current( TRACKED_VARS *tracked, int32 node_key_cell, CTDB_MES_NODE_KEY node_key) { CTDB_MES_NODE_KEY tmp; int32 i,look_ahead,look_back, cell; for (i=0,look_ahead=1,look_back=1;(look_ahead || look_back);i++) { if ( look_ahead ) { look_ahead = tracked_node_key_look_ahead(tracked,i,&cell,&tmp); if (look_ahead && (tmp == node_key) && (cell == node_key_cell)) { tracked->node_keys.current = (tracked->node_keys.current+i)% TRACKED_MAX_NODE_LIST_LENGTH; return; } } if ( look_back ) { look_back = tracked_node_key_look_back(tracked,i,&cell,&tmp); if ( look_back && (tmp == node_key) && (cell == node_key_cell)) { tracked->node_keys.current = ((tracked->node_keys.current-i+ TRACKED_MAX_NODE_LIST_LENGTH)% TRACKED_MAX_NODE_LIST_LENGTH); return; } } } /* Failed to find the key in the list */ tracked_clear_node_key_list(tracked, node_key_cell, node_key); } static void tracked_create( int32 vehicle_id, int32 user_data_handle, TRACKED_PARAMETRIC_DATA *params) { TRACKED_VARS *tracked = (TRACKED_VARS *)STDALLOC(sizeof(TRACKED_VARS)); float64 direction[XYZC]; bzero(tracked, sizeof(TRACKED_VARS)); class_set_user_data((CLASS_USER_DATA_TYPE)vtab_get_vehicle(vehicle_id), user_data_handle, (CLASS_USER_DATA_TYPE)tracked); tracked->soil_latch = -1; if (params) tracked->params = params; else tracked->params = &tracked_dummy_params; /* this is a hack. since we cannot find out the vehicles * current direction in di_create like we used to (we * shouldn't be calling another subclass's functions during * our subclasses create routine).. so, we initialize `cells_initted' * to FALSE in our create routine indicating that it hasn't yet * been initialized. during the first tick, it will be * initialized. */ tracked->cells_initted = FALSE; tracked->desired_dir[CELL3D] = -1; tracked->saved_position[CELL3D] = -1; /* Remember this for use by HULLS_GET_LIMITS, etc. */ tracked->speed = 0.0; tracked->desired_gear = HULLS_GEAR_FORWARD; tracked->state = TRACKED_STATE_HEALTHY; tracked->current_gear = HULLS_GEAR_FORWARD; tracked->node_look_fcn = (TRACKED_NODE_KEY_LOOK_FCN)tracked_node_key_look_ahead; tracked_set_current(tracked, -1, ent_get_mes_key(vehicle_id)); /* * Register callback to tick during the appropriate phase at the * appropriate rate */ callback_register_handler( disdbobj_get_tick_event(vehicle_id, DISDBOBJ_PHYSICAL_HULL_MODELING, 0), tracked_tick, (ADDRESS) NULL); tracked->ho_event_registration = ho_register_handlers( vehicle_id, NULL, NULL, NULL, NULL, trk_started_being_moved, NULL, trk_stopped_being_moved, NULL); } static void tracked_destroy( int32 vehicle_id, int32 user_data_handle) { TRACKED_VARS *tracked = (TRACKED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), user_data_handle); if (!tracked) /* Passive error detection */ return; ho_unregister_handlers(&tracked->ho_event_registration); class_set_user_data((CLASS_USER_DATA_TYPE)vtab_get_vehicle(vehicle_id), user_data_handle, (CLASS_USER_DATA_TYPE)0); STDDEALLOC(tracked); } static void set_dir_speed( int32 vehicle_id, int32 user_data_handle, struct hulls_set_direction_speed *data) { TRACKED_VARS *tracked = (TRACKED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), user_data_handle); if (!tracked) /* Passive error detection */ return; tracked->control_state = TRACKED_EXPLICIT; tracked->desired_dir[X] = data->direction[X]; tracked->desired_dir[Y] = data->direction[Y]; tracked->desired_dir[CELL3D] = data->cell; if (data->speed > 0.0) { tracked->desired_gear = HULLS_GEAR_FORWARD; tracked->speed = data->speed; } else { tracked->desired_gear = HULLS_GEAR_REVERSE; tracked->speed = -data->speed; } tracked->max_turn = data->max_turn_rate; tracked->max_accel = data->max_accel; tracked->last_command_time = time_last_simulation_clock; } static void set_vel_gear( int32 vehicle_id, int32 user_data_handle, struct hulls_set_velocity_gear *data) { float64 mag; TRACKED_VARS *tracked = (TRACKED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), user_data_handle); if (!tracked) /* Passive error detection */ return; tracked->control_state = TRACKED_EXPLICIT; if (data->velocity[X] || data->velocity[Y]) { tracked->desired_dir[CELL3D] = data->cell; if (data->gear == HULLS_GEAR_FORWARD) { tracked->desired_dir[X] = data->velocity[X]; tracked->desired_dir[Y] = data->velocity[Y]; } else { tracked->desired_dir[X] = -data->velocity[X]; tracked->desired_dir[Y] = -data->velocity[Y]; } mag = (data->velocity[X] * data->velocity[X] + data->velocity[Y] * data->velocity[Y] + data->velocity[Z] * data->velocity[Z]); /* Often the speed will not change between calls */ if (mag != (tracked->speed * tracked->speed)) tracked->speed = fsqrt(mag); } else tracked->speed = 0.0; tracked->desired_gear = data->gear; tracked->max_turn = data->max_turn_rate; tracked->max_accel = data->max_accel; tracked->last_command_time = time_last_simulation_clock; } static void set_vel_dir( int32 vehicle_id, int32 user_data_handle, struct hulls_set_velocity_direction *data) { float64 mag; TRACKED_VARS *tracked = (TRACKED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), user_data_handle); if (!tracked) /* Passive error detection */ return; tracked->control_state = TRACKED_EXPLICIT; tracked->desired_dir[X] = data->direction[X]; tracked->desired_dir[Y] = data->direction[Y]; tracked->desired_dir[CELL3D] = data->cell; mag = (data->velocity[X] * data->velocity[X] + data->velocity[Y] * data->velocity[Y] + data->velocity[Z] * data->velocity[Z]); /* Often the speed will not change between calls */ if (mag != (tracked->speed * tracked->speed)) tracked->speed = fsqrt(mag); if ((data->velocity[X] * data->direction[X] + data->velocity[Y] * data->direction[Y]) < 0.0) tracked->desired_gear = HULLS_GEAR_REVERSE; else tracked->desired_gear = HULLS_GEAR_FORWARD; if (data->max_turn_rates) tracked->max_turn = data->max_turn_rates[0]; else tracked->max_turn = 0.0; tracked->max_accel = data->max_accel; tracked->last_command_time = time_last_simulation_clock; } static void set_external_control( int32 vehicle_id, int32 user_data_handle, struct hulls_set_velocity_direction *data) { TRACKED_VARS *tracked = (TRACKED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), user_data_handle); if (!tracked) /* Passive error detection */ return; set_vel_dir(vehicle_id, user_data_handle, data); tracked->control_state = TRACKED_EXTERNAL; tracked->position[X] = data->position[X]; tracked->position[Y] = data->position[Y]; tracked->position[CELL3D] = data->cell; tracked->last_command_time = time_last_simulation_clock; } static void set_vel_ori( int32 vehicle_id, int32 user_data_handle, struct hulls_set_velocity_orientation *data) { float64 mag; TRACKED_VARS *tracked = (TRACKED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), user_data_handle); if (!tracked) /* Passive error detection */ return; tracked->control_state = TRACKED_EXPLICIT; tracked->desired_dir[X] = data->velocity[X]; tracked->desired_dir[Y] = data->velocity[Y]; tracked->desired_dir[CELL3D] = data->cell; mag = (data->velocity[X] * data->velocity[X] + data->velocity[Y] * data->velocity[Y] + data->velocity[Z] * data->velocity[Z]); /* Often the speed will not change between calls */ if (mag != (tracked->speed * tracked->speed)) tracked->speed = fsqrt(mag); if (data->orientation && ((data->velocity[X] * cos(data->orientation[0]) + data->velocity[Y] * sin(data->orientation[0])) < 0.0)) tracked->desired_gear = HULLS_GEAR_REVERSE; else tracked->desired_gear = HULLS_GEAR_FORWARD; if (data->max_turn_rates) tracked->max_turn = data->max_turn_rates[0]; else tracked->max_turn = 0.0; tracked->max_accel = data->max_accel; tracked->last_command_time = time_last_simulation_clock; } static void set_pos_dir( int32 vehicle_id, int32 user_data_handle, struct hulls_set_position_direction *data) { TRACKED_VARS *tracked = (TRACKED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), user_data_handle); if (!tracked) /* Passive error detection */ return; tracked->control_state = TRACKED_POSITION_DIRECTION; tracked->position[X] = data->position[X]; tracked->position[Y] = data->position[Y]; tracked->position[CELL3D] = data->cell; tracked->direction[X] = data->direction[X]; tracked->direction[Y] = data->direction[Y]; tracked->direction[CELL3D] = data->cell; tracked->last_command_time = time_last_simulation_clock; } static void set_goal_corr( int32 vehicle_id, int32 user_data_handle, struct hulls_set_goal_corridor *data) { TRACKED_VARS *tracked = (TRACKED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), user_data_handle); if (!tracked) /* Passive error detection */ return; tracked->control_state = TRACKED_GOAL_CORRIDOR; tracked->position[X] = data->position[X]; tracked->position[Y] = data->position[Y]; tracked->position[CELL3D] = data->cell; tracked->direction[X] = data->direction[X]; tracked->direction[Y] = data->direction[Y]; tracked->direction[CELL3D] = data->cell; tracked->approach_speed = data->approach_speed; tracked->corridor_width = data->corridor_width; tracked->last_command_time = time_last_simulation_clock; } static void set_target_id( int32 vehicle_id, int32 user_data_handle, struct hulls_set_target_id *data) { TRACKED_VARS *tracked = (TRACKED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), user_data_handle); if (!tracked) /* Passive error detection */ return; tracked->control_state = TRACKED_TARGET_ID; tracked->target_id = data->id; tracked->last_command_time = time_last_simulation_clock; } static void set_target_position( int32 vehicle_id, int32 user_data_handle, struct hulls_set_target_position *data) { TRACKED_VARS *tracked = (TRACKED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), user_data_handle); if (!tracked) /* Passive error detection */ return; tracked->control_state = TRACKED_TARGET_POSITION; tracked->position[X] = data->position[X]; tracked->position[Y] = data->position[Y]; tracked->position[CELL3D] = data->cell; tracked->last_command_time = time_last_simulation_clock; } static void get_eta( int32 vehicle_id, int32 user_data_handle, struct hulls_get_eta *data) { TRACKED_VARS *tracked = (TRACKED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), user_data_handle); if (!tracked) /* Passive error detection */ return; data->eta = tracked_compute_eta(vehicle_id, tracked, data->position, data->cell); } static void set_takeoff( int32 vehicle_id, int32 user_data_handle, struct hulls_set_takeoff *data) { float64 position[XYZC]; TRACKED_VARS *tracked = (TRACKED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), user_data_handle); if (!tracked) /* Passive error detection */ return; ent_get_position_gcs(vehicle_id, position); position[Z] = data->altitude_agl; ent_set_position_gcs(vehicle_id, position); } static void set_fly_level( int32 vehicle_id, int32 user_data_handle, struct hulls_set_fly_level *data) { struct hulls_set_velocity_direction set_vel_dir_data; TRACKED_VARS *tracked = (TRACKED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), user_data_handle); if (!tracked) /* Passive error detection */ return; /* Just call set_vel_dir routine - this routine does not really make sense for a tracked hull. */ set_vel_dir_data.velocity[X] = cos(data->speed); set_vel_dir_data.velocity[Y] = sin(data->speed); set_vel_dir_data.direction[X] = data->track[X]; set_vel_dir_data.direction[Y] = data->track[Y]; set_vel_dir_data.cell = data->cell; set_vel_dir_data.max_turn_rates = data->max_turn_rates; set_vel_dir(vehicle_id, user_data_handle, &set_vel_dir_data); tracked->last_command_time = time_last_simulation_clock; } static void get_max_range( int32 vehicle_id, int32 user_data_handle, struct hulls_get_max_range *data) { float64 fuel_available; float64 speed; TRACKED_VARS *tracked = (TRACKED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), user_data_handle); if (!tracked) /* Passive error detection */ { data->max_range = 0.0; return; } if ((fuel_available = scmp_get_amount(vehicle_id, munition_Fuel, 0)) != 0.0) { speed = ent_get_speed(vehicle_id); data->max_range = fuel_available * tracked_fuel_usage_rate (tracked->params, speed); } else { data->max_range = 0.0; } } static void get_limits( int32 vehicle_id, int32 user_data_handle, struct hulls_get_limits *data) { float64 pos[XYZC], dir[XYZC], htow[3][3], dz; int32 soil_type; struct tracked_soils *soil; TRACKED_VARS *tracked = (TRACKED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), user_data_handle); CTDB *ctdb = ent_get_ctdb(vehicle_id); int32 cell =ent_get_cell(vehicle_id); if (data->position) { /* check to make sure it is with respect to the correct cell.. */ if (data->cell != cell) gcs_gcs3_to_gcscs64(data->cell, data->position, cell, pos); else VMAT3_VEC_COPY64(data->position, pos); pos[CELL3D] = cell; } else ent_get_position_gcs(vehicle_id, pos); if (data->direction) { float64 tmp_dir[XYZ]; /* have to make sure that data->cell is the same as the * entities current cell. if not, we have to convert.. */ if (data->cell != cell) { gcs_vec_to_vec64(data->cell, data->direction, cell, tmp_dir); } else { VMAT2_VEC_COPY64(data->direction, tmp_dir); } /* Get a 3D direction vector at that location */ /* Big-wheel at that location */ pos[Z] += 1.0; /* safety margin for multi-level */ ctdb_place_vehicle(ctdb, pos[X], pos[Y], 1.0, 1.0, tmp_dir[X], tmp_dir[Y], pos+Z, htow, &soil_type); dz = htow[Y][Z]; } else { ent_get_direction_gcs(vehicle_id, dir); dz = dir[Z]; pos[Z] += 1.0; /* safety margin for multi-level */ } soil_type = CTDB_SIMNET_SOIL(ctdb_lookup_soil_ml(ctdb, pos[X], pos[Y], pos[Z])); /* If we're on a bridge, use the default soil type. */ if(tracked->bridge_id) soil = tracked->params->soils; else soil = tracked->params->soils + tracked_find_soil(soil_type, tracked); /* Adjust for grade (lifted from trk_tick.c) */ if(!tracked->bridge_id) { if (dz > soil->max_climb_sin) /* Too steep */ data->max_speed = 0.0; else if (dz <= 0.0) /* Downhill */ data->max_speed = soil->max_speeds_mps[TRACKED_MAX_FORWARD]; else data->max_speed = (soil->max_speeds_mps[TRACKED_MAX_FORWARD] * (1.0 - sqrt(dz / soil->max_climb_sin))); } else data->max_speed = soil->max_speeds_mps[TRACKED_MAX_FORWARD]; data->max_accel = soil->max_accel_mps2; data->max_decel = soil->max_decel_mps2; data->max_turn = soil->max_turn_rps; data->min_radius = 0.0; /* Tracked vehicles can pivot steer */ data->max_turn_accel = 0.0; /* No Maximum */ data->max_sideways = 0.0; /* can't move sideways */ data->max_up = 0.0; /* can't fly */ } static void trk_started_being_moved( int32 vehicle_id, int32 vehicle_moving_me, int32 dynamics, ADDRESS user_data) { TRACKED_VARS *tracked = (TRACKED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), tracked_user_data_handle); if (!tracked) return; tracked->being_moved = TRUE; } static void trk_stopped_being_moved( int32 vehicle_id, int32 vehicle_moving_me, float64 *final_pos, ADDRESS user_data) { TRACKED_VARS *tracked = (TRACKED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), tracked_user_data_handle); if (!tracked) return; tracked->being_moved = FALSE; ent_get_direction_gcs(vehicle_id, tracked->direction); } static void get_trouble_state( int32 vehicle_id, int32 user_data_handle, struct hulls_get_trouble_state *data) { TRACKED_VARS *tracked = (TRACKED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), tracked_user_data_handle); if(!tracked) { data->in_trouble_state = FALSE; return; } if(tracked->state == TRACKED_STATE_IN_TROUBLE) data->in_trouble_state = TRUE; else data->in_trouble_state = FALSE; } int32 tracked_node_key_look_ahead( TRACKED_VARS *tracked, uint32 how_far, int32 *node_key_cell, CTDB_MES_NODE_KEY *node_key) { int32 index, last; if (!how_far) { *node_key_cell = tracked->node_keys.list_cell[tracked->node_keys.current]; *node_key = tracked->node_keys.list[tracked->node_keys.current]; return 1; } if (tracked->node_keys.last <= tracked->node_keys.current ) last = tracked->node_keys.last + TRACKED_MAX_NODE_LIST_LENGTH; else last = tracked->node_keys.last; index = tracked->node_keys.current + how_far; if ( index <= last ) { *node_key_cell = tracked->node_keys.list_cell[index%TRACKED_MAX_NODE_LIST_LENGTH]; *node_key =tracked->node_keys.list[index%TRACKED_MAX_NODE_LIST_LENGTH]; return 1; } *node_key_cell = GCS_ILLEGAL_CELL; *node_key = CTDB_INVALID_MES_NODE_KEY; return 0; } int32 tracked_node_key_look_back( TRACKED_VARS *tracked, uint32 how_far, int32 *node_key_cell, CTDB_MES_NODE_KEY *node_key) { int32 index, current; if (!how_far) { *node_key_cell = tracked->node_keys.list_cell[tracked->node_keys.current]; *node_key = tracked->node_keys.list[tracked->node_keys.current]; return 1; } if (tracked->node_keys.first >= tracked->node_keys.current) current = tracked->node_keys.current + TRACKED_MAX_NODE_LIST_LENGTH; else current = tracked->node_keys.current; index = current - how_far; if ( index >= tracked->node_keys.first) { *node_key_cell = tracked->node_keys.list_cell[index%TRACKED_MAX_NODE_LIST_LENGTH]; *node_key=tracked->node_keys.list[index%TRACKED_MAX_NODE_LIST_LENGTH]; return 1; } *node_key_cell = GCS_ILLEGAL_CELL; *node_key = CTDB_INVALID_MES_NODE_KEY; return 0; } void tracked_add_node_key_to_list( TRACKED_VARS *tracked, int32 node_key_cell, CTDB_MES_NODE_KEY node_key) { /* Only add the key if it is not equal to the current node key */ if (CTDB_MES_NODE_KEYS_EQUAL(tracked->node_keys.list[tracked->node_keys. current], node_key) && (tracked->node_keys.list_cell[tracked->node_keys.current] == node_key_cell)) return; tracked->node_keys.last = (tracked->node_keys.current+1)% TRACKED_MAX_NODE_LIST_LENGTH; tracked->node_keys.list_cell[tracked->node_keys.last] = node_key_cell; tracked->node_keys.list[tracked->node_keys.last] = node_key; if (tracked->node_keys.last == tracked->node_keys.first) tracked->node_keys.first = (tracked->node_keys.first+1)% TRACKED_MAX_NODE_LIST_LENGTH; } void tracked_clear_node_key_list( TRACKED_VARS *tracked, int32 seed_cell, CTDB_MES_NODE_KEY seed) { tracked->node_keys.current = 0; tracked->node_keys.last = 0; tracked->node_keys.first = 0; tracked->node_keys.list_cell[0] = seed_cell; tracked->node_keys.list[0] = seed; } static void set_mes_key( int32 vehicle_id, int32 user_data_handle, struct hulls_set_mes_key *data) { TRACKED_VARS *tracked = (TRACKED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), user_data_handle); if (!tracked) /* Passive error detection */ return; DEBUG_TRACKED(("cur %d %d %d, next %d %d %d\n", CTDB_MES_NODE_KEY_TO_MES_ID(data->mes_key), CTDB_MES_NODE_KEY_TO_ENCLOSURE_ID(data->mes_key), data->mes_key_cell, CTDB_MES_NODE_KEY_TO_MES_ID(data->next_mes_key), CTDB_MES_NODE_KEY_TO_ENCLOSURE_ID(data->next_mes_key), data->next_mes_key_cell)); tracked_set_current(tracked, data->mes_key_cell, data->mes_key); tracked_add_node_key_to_list(tracked, data->next_mes_key_cell, data->next_mes_key); DEBUG_TRACKED_EXEC(tracked_print_node_key_list(tracked)); } static void get_mes_key( int32 vehicle_id, int32 user_data_handle, struct hulls_get_mes_key *data) { TRACKED_VARS *tracked = (TRACKED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), user_data_handle); if (!tracked) /* Passive error detection */ { data->mes_key_cell = GCS_ILLEGAL_CELL; data->mes_key = CTDB_INVALID_MES_NODE_KEY; data->next_mes_key_cell = GCS_ILLEGAL_CELL; data->next_mes_key = CTDB_INVALID_MES_NODE_KEY; } else { tracked_node_key_look_ahead(tracked, 0, &data->mes_key_cell, &data->mes_key); tracked_node_key_look_ahead(tracked, 1, &data->next_mes_key_cell, &data->next_mes_key); } }