/**************************************************************************** * File: trk_sink.c * * * Copyright 1996 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: For Sinking ships. * Created: Tue Jan 28 11:03:54 EST 1997 * Author: skukolic * $Revision$ * * Remarks: * * * ****************************************************************************/ /* For organizational purposes, this file is split into 4 sections: */ /* INCLUDES AND DECLARATIONS: */ /* READER FILE STUFF */ /* MODSAF INTERFACE */ /* THIS SINK MODEL: */ /* SINK MODEL TEST PROGRAM: */ /**************************************************************/ /**************************************************************/ /* INCLUDES AND DECLARATIONS: */ /**************************************************************/ #include #ifdef TRK_SINK_TEST #define TRACKED_SINK_TEST #endif /*TRK_SINK_TEST*/ #ifdef TRACKED_SINK_TEST #ifndef NO_MODSAF #define NO_MODSAF #endif /*not NO_MODSAF*/ #endif /*TRACKED_SINK_TEST*/ #ifndef NO_MODSAF #ifdef NO_READER #undef NO_READER #endif #define STATIC static /* ModSAF includes */ #include "libtrk_local.h" #include #include #include #include #include #include #include #include /*#include */ /*#include "veh_type.h"*/ #endif /*not NO_MODSAF*/ #ifdef NO_MODSAF /* need types so that we can compile this file standalone*/ /* STATIC identifies public functions for non-ModSAF Apps */ #define STATIC #ifndef NO_READER #include #include #else typedef int int32; typedef float float32; typedef double float64; #endif #ifndef PI #define PI 3.14159265358979323844 #endif #endif /*NO_MODSAF*/ /**************************************************************/ /* local SINK model defines, and structs */ /* physical constants: */ #define WATER_DENSITY 1000.0 /* kg/m^3 */ #define GRAVITY 9.8 /* m/s^2 */ typedef struct tracked_sink_vars { /* variables marked C are critical * for getting state from time */ float64 started; /* VEHICLE DEPENDANT CONSTANTS */ /* for mass model */ float64 len[3]; /* width, length, height */ float64 mass0; /*C mass with no water */ float64 max_fill_fraction; /*C fraction of width*length*height * availible for water */ float64 fill_fraction_rate;/*C (d/dt) fill_fraction * should be computed from * damage, but no time */ /* for sinking model */ float64 drag_multiplier; /* multiplicative drag fudge * factor */ /* for orientation model */ float64 rp_transition[2]; /*C target roll, pitch angles * for t_transition */ float64 rp_final[2]; /*C target roll, pitch angles * for t_final */ float64 rp_max_rate[2]; /*C max time derivative of roll * and pitch angles */ /* z extra */ float64 extra_z_floor; /*C effective_z_floor = z_floor_external + this*/ float64 z_offset; /*C z_out = z_bottom + z_offset */ /* COMPUTED CONSTANTS */ /* for mass model */ float64 mass_water; /*C total_volume * WATER_DENSITY*/ float64 fill_fraction_critical; /* fraction at transition to sinking*/ /* SITUATION DEPENDENT CONSTANTS */ /* z extra */ float64 z_ocean, z_floor; /* z pos of ocean, & hard floor*/ /* for sinking model */ float64 drag_coeff; /* force_drag = drag_coeff*vel**2 */ /* drag_coeff = 2*water_density*Area*fudge */ float64 dzdt_max; /*C max |dz/dt| */ float64 t_const; /*C integration constant for dz/dt */ float64 z_const; /*C integration constant for z(t) */ /* computed times */ float64 t_transition; /*C when transition to sinking */ float64 t_final; /*C when hit bottom */ /* DYNAMICAL VARIABLES */ float64 t_previous; /*C time at which vars were valid */ /* mass model */ /*float64 fill_fraction;*/ /* how much of the vehicle's volume is * currently filled with water */ /*float64 mass;*/ /* total mass, including water */ /*float64 is_sinking;*/ /* 0 => no (taking water), 1 => yes*/ /* sinking model (or during mass model) */ float64 z, dzdt; /* z position and z velocity */ /* orientation model */ float64 roll_pitch[2]; /*C roll, pitch */ } TRACKED_SINK_VARS; /**************************************************************/ /* SINK model functions: */ #ifndef NO_READER /* read the trk_sea_veh.rdr file, store the table into trk_sea_veh_ru, * and do some syntax checking of the table */ STATIC int32 trk_sea_veh_ru_read( char *data_path, uint32 flags); /* Find the reader union describing a vehicle's sea variables. * Search for vehicle description which has a pair matching * field_name, field_value. */ STATIC READER_UNION *trk_sea_get_veh_ru( char *field_name, int field_value_type, /* READER_INTEGER or READER_CHARPTR */ READER_UNION field_value); /* Fill in invalid values */ STATIC int32 tracked_sink_get_veh_invalid( TRACKED_SINK_VARS *sink); /* Fill in the initial sink fields from the vehicle description, from * the reader file */ STATIC int32 tracked_sink_get_veh( READER_UNION *veh, TRACKED_SINK_VARS *sink); /* Fill in defaults for fields not already filled in */ STATIC int32 tracked_sink_get_veh_defaults( TRACKED_SINK_VARS *sink); #endif /*NO_READER*/ #ifndef NO_MODSAF int32 tracked_sink_init( char *data_path, uint32 flags); void tracked_sink_add( int32 vehicle_id, TRACKED_VARS *tracked, float64 dt); /* Fill in the sink mass and lenght fields from physdb, * if they have not already been filled in */ static int32 tracked_sink_get_veh_dims( int32 vehicle_id, TRACKED_SINK_VARS *sink); /* Test to see if over water */ static int32 tracked_over_water( float64 pos[XYZC], int32 vehicle_id); /* Get sea-level, and hard floor z positions */ static int32 tracked_sink_get_z_ocean_z_floor( float64 pos[XYZC], int32 vehicle_id, float64 *z_ocean, float64 *z_floor); #endif /*not NO_MODSAF*/ /* tracked_sink_start() * After the vehicle-constants have been filled in, * this computes the rest the situation-dependant constants, * and the initial state. */ STATIC int32 tracked_sink_start( TRACKED_SINK_VARS *sink, float64 z_ocean, float64 z_floor, float64 time, float64 fill_fraction, /* can be zero */ float64 z, /* z, and dzdt are only needed if already sinking */ float64 dzdt, float64 roll, float64 pitch, float64 final_roll, /* if > -7 radians then overrides defaults */ float64 final_pitch);/* if > -7 radians then overrides defaults */ /* tracked_sink_state() * Compute sink state at a later time */ STATIC int32 tracked_sink_state( TRACKED_SINK_VARS *sink, float64 time, float64 *fill_fraction, float64 *mass, float64 *is_sinking, float64 *z, float64 *dzdt, float64 *roll, float64 *pitch); static float64 arctanh(float64 y); static float64 log_cosh(float64 x); static float64 inv_log_cosh(float64 y); /**************************************************************/ /**************************************************************/ /* READER FILE STUFF */ /**************************************************************/ #ifndef NO_READER static READER_UNION trk_sea_veh_ru; /* read the trk_sea_veh_.rdr file, store the table into trk_sea_veh_ru, * and do some syntax checking of the table. */ STATIC int32 trk_sea_veh_ru_read( char *data_path, uint32 flags) { int32 rdr_error; char *trk_sea_veh_filename = "trk_sea_veh.rdr"; int32 i_len, i, j_len, j, k_len; READER_UNION *veh, *field; /* Read from the trk_sea_veh.rdr file: */ if (rdr_error = reader_read(trk_sea_veh_filename, data_path, &trk_sea_veh_ru, flags | READER_TYPING)) { if (rdr_error == READER_READ_ERROR) fprintf(stderr, "Syntax error in %s\n", trk_sea_veh_filename); else fprintf(stderr, "Unable to open %s for defaults, aborting.\n", trk_sea_veh_filename); return(0); } /* check the table format, * should be * ::= ({}*) * ::= ({}*) * ::= ( [ | ]) */ i_len = trk_sea_veh_ru.array[0].integer - 1; for (i=0; i < i_len; i++) { if ((READER_ARRAY != READER_UTYPE(trk_sea_veh_ru.array, i+1)) ||(0 > (j_len = (veh = trk_sea_veh_ru.array[i+1].array)[0].integer - 1))) { fprintf(stderr, "Bad table format in \"%s\"a, veh_entry %d\n", trk_sea_veh_filename, i); return(0); } for (j=0; j < j_len; j++) { if ((READER_ARRAY != READER_UTYPE(veh, j+1)) ||(3 != (k_len = (field = veh[j+1].array)[0].integer)) ||(READER_CHARPTR != READER_UTYPE(field, 1)) ||(!( (READER_CHARPTR == READER_UTYPE(field, 2)) ||(READER_INTEGER == READER_UTYPE(field, 2)) ||(READER_REAL == READER_UTYPE(field, 2))))) { fprintf(stderr, "Bad table format in \"%s\"a, veh_entry %d, field %d\n", trk_sea_veh_filename, i, j); return(0); } } } return(1); } /* Find the reader union describing a vehicle's sea variables. * Search for vehicle description which has a pair matching * field_name, field_value. */ STATIC READER_UNION *trk_sea_get_veh_ru( char *field_name, int field_value_type, /* READER_INTEGER or READER_CHARPTR */ READER_UNION field_value) { int i_len, i; READER_UNION *veh, *field_pair; field_name = reader_get_symbol(field_name); i_len = trk_sea_veh_ru.array[0].integer - 1; for (i=0; iFIELD))/sizeof(float64)) { "mass", AOFFSET(mass0)}, { "width", AOFFSET(len[0])}, { "length", AOFFSET(len[1])}, { "height", AOFFSET(len[2])}, { "max_fill_fraction", AOFFSET(max_fill_fraction) }, { "fill_fraction_rate", AOFFSET(fill_fraction_rate)}, { "drag_multiplier", AOFFSET(drag_multiplier)}, { "transition_roll_angle", AOFFSET(rp_transition[0])}, { "transition_pitch_angle",AOFFSET(rp_transition[1])}, { "final_roll_angle", AOFFSET(rp_final[0]) }, { "final_pitch_angle", AOFFSET(rp_final[1]) }, { "max_roll_rate", AOFFSET(rp_max_rate[0])}, { "max_pitch_rate", AOFFSET(rp_max_rate[1])}, { "extra_z_floor", AOFFSET(extra_z_floor) }, { "extra_z_out_offset", AOFFSET(z_offset) }, { 0, 0 } }; static int tracked_sink_veh_fields_inited = 0; #define TRK_SEA_FIELD_INVALID (-98765.) /* Fill in invalid values */ STATIC int32 tracked_sink_get_veh_invalid( TRACKED_SINK_VARS *sink) { float64 *sinka, *sinkb; sinkb = (float64*)(&(sink->z_offset)); for (sinka = (float64*)(&(sink->len[0])); sinka <= sinkb; ++sinka) *sinka = TRK_SEA_FIELD_INVALID; return(1); } /* Fill in the initial sink fields from the vehicle description, from * the reader file */ STATIC int32 tracked_sink_get_veh( READER_UNION *veh, TRACKED_SINK_VARS *sink) { READER_UNION *field; struct veh_fields *fieldsp; float64 *sinka; if (!tracked_sink_veh_fields_inited) { /* convert veh_fields strings into reader symbols */ for (fieldsp = tracked_sink_veh_fields; fieldsp->sym; ++fieldsp) fieldsp->sym = reader_get_symbol(fieldsp->sym); tracked_sink_veh_fields_inited = 1; } sinka = (float64*)(sink); for (fieldsp = tracked_sink_veh_fields; fieldsp->sym; ++fieldsp) { if ((field = reader_find_tag(fieldsp->sym, veh, READER_UNTAGGED, READER_NO_ERRORS)) &&(field[0].integer >= 3)) { if (READER_UTYPE(field, 2)== READER_REAL) sinka[fieldsp->offset] = field[2].real; else if (READER_UTYPE(field, 2)== READER_INTEGER) sinka[fieldsp->offset] = field[2].integer; } } return(1); } /* Fill in defaults for fields not already filled in */ STATIC int32 tracked_sink_get_veh_defaults( TRACKED_SINK_VARS *sink) { #define IF_SET(VAR, VALUE) \ if (TRK_SEA_FIELD_INVALID == (VAR)) (VAR) = (VALUE) IF_SET( sink->max_fill_fraction, 0.85 ); IF_SET( sink->fill_fraction_rate, 1.0/(log(sink->mass0)/log(10.) + 2) ); IF_SET( sink->drag_multiplier, 1.0 ); IF_SET( sink->rp_transition[0], 0.6 ); IF_SET( sink->rp_transition[1], 0.2 ); IF_SET( sink->rp_final[0], 1.0 ); IF_SET( sink->rp_final[1], 0.1 ); IF_SET( sink->rp_max_rate[0], sink->fill_fraction_rate ); IF_SET( sink->rp_max_rate[1], 0.5 * sink->fill_fraction_rate ); IF_SET( sink->extra_z_floor, 0.); IF_SET( sink->z_offset, 0.); return(1); } #endif /*not NO_READER*/ /**************************************************************/ /**************************************************************/ /* MODSAF INTERFACE */ /**************************************************************/ #ifndef NO_MODSAF int32 tracked_sink_init( char *data_path, uint32 flags) { /* read the file trk_sea_veh.rdr */ if (!trk_sea_veh_ru_read(data_path, flags)) return(0); return(1); } /**************************************************************/ /* tracked_sink_add() etc. */ void tracked_sink_add( int32 vehicle_id, TRACKED_VARS *tracked, float64 dt) { TRACKED_SINK_VARS *sink; float64 pos[XYZC]; float64 vel[XYZC]; float64 rot[XYZ][XYZ]; float32 yaw32, pitch32, roll32; float64 yaw, pitch, roll; ObjectType my_guise; float64 time; float64 fill_fraction, mass, is_sinking; READER_UNION guise_ru, *veh_ru; int32 cell = ent_get_cell(vehicle_id); sink = (TRACKED_SINK_VARS*)(&(tracked->sink_state[0])); if (sink->started && (sink->t_previous >= sink->t_final)) return; ent_get_position_gcs(vehicle_id, pos); if (!tracked_over_water(pos, vehicle_id)) return; ent_get_velocity_gcs(vehicle_id, vel); ent_get_rotation_gcs(vehicle_id, rot, NULL); vmat3_rotation64_to_zdown_angles32(rot, &yaw32, &pitch32, &roll32); yaw = yaw32; pitch = pitch32; roll = roll32; guise_ru.integer = my_guise = ent_get_guise(vehicle_id, 0); time = 0.001*time_last_simulation_clock; if (!sink->started) { sink->started = 1; /* compute z_ocean, z_floor */ tracked_sink_get_z_ocean_z_floor(pos, vehicle_id, &sink->z_ocean, &sink->z_floor); tracked_sink_get_veh_invalid(sink); if (0!=(veh_ru = trk_sea_get_veh_ru("guise", READER_INTEGER, guise_ru))) tracked_sink_get_veh(veh_ru, sink); /*from reader file entry*/ tracked_sink_get_veh_dims(vehicle_id, sink); /* from physdb */ tracked_sink_get_veh_defaults(sink); /* computed */ tracked_sink_start(sink, sink->z_ocean, sink->z_floor, time, 0., pos[Z], vel[Z], roll, pitch, -7., -7.); } tracked_sink_state(sink, time, &fill_fraction, &mass, &is_sinking, &pos[Z], &vel[Z], &roll, &pitch); if (vehicle_id == tracked_rao_veh_id_debug) printf("veh %d: t= %lg, z= %lg, vz= %lg, pitch= %lg, roll= %lg\n", vehicle_id, time, pos[Z], vel[Z], pitch, roll); vmat3_zdown_angles32_to_rotation64(yaw, pitch, roll, rot); ent_set_rotation_gcs(vehicle_id, rot, cell); ent_set_velocity_gcs(vehicle_id, vel); ent_set_position_gcs(vehicle_id, pos); } /* Fill in the sink mass and lenght fields from physdb, * if they have not already been filled in */ static int32 tracked_sink_get_veh_dims( int32 vehicle_id, TRACKED_SINK_VARS *sink) { PHYSDB_DATA *pdb; int32 i; /* first get data from physdb: */ pdb = ent_get_physdb(vehicle_id); if (sink->mass0 <= 0) sink->mass0 = pdb->mass_kg; for (i=0; i<3; i++) if (sink->len[i] <=0) sink->len[i] = pdb->dimensions[i]; /*sink->waterline = pdb->model_base_adjustment;*/ return (1); } /* Test to see if over water */ static int32 tracked_over_water( float64 pos[XYZC], int32 vehicle_id) { CTDB *ctdb; CTDB_ELEV_DATA maxelev; static CTDB_ELEV_QUALIFIER qual=(CTDB_INCL_WATER); ctdb = ent_get_ctdb(vehicle_id); ctdb_lookup_qual_elevation(ctdb, pos[X], pos[Y], qual, CTDB_NO_REF_ELEV, &maxelev); if (maxelev.tdbclass == CTDB_FC_ILLEGAL) return(0); return(1); } /* Get sea-level, and hard floor z positions */ static int32 tracked_sink_get_z_ocean_z_floor( float64 pos[XYZC], int32 vehicle_id, float64 *z_ocean, float64 *z_floor) { CTDB *ctdb; CTDB_ELEV_DATA elev[CTDB_MAX_ELEVS]; int32 water_found, ground_found, eidx; ENV_INTERFACE ei; ctdb = ent_get_ctdb(vehicle_id); ctdb_lookup_elevation_mes(ctdb, pos[X], pos[Y], CTDB_USE_DEFAULT, elev); water_found = ground_found = 0; for (eidx = 0; eidx < CTDB_MAX_ELEVS; ++eidx) { if (elev[eidx].tdbclass == CTDB_FC_ILLEGAL) /* denotes end */ break; if (elev[eidx].tdbclass == CTDB_FC_MICRO) { if (elev[eidx].subclass == CTDB_FS_SKIN) { *z_floor = elev[eidx].z; ground_found = 1; } else if (elev[eidx].subclass == CTDB_FS_WATER) { *z_ocean = elev[eidx].z; water_found = 1; } } if (water_found && ground_found) break; } if (!(water_found && ground_found)) return(0); bzero(&ei, sizeof(ENV_INTERFACE)); bcopy(pos, &ei.u.sea_tide_level.input_vector, 4*sizeof(float64)); if (env_get(ENV_SEA_TIDE_LEVEL, &ei) != -1) *z_ocean += ei.u.sea_tide_level.output_value; return(1); } #endif /*not NO_MODSAF*/ /**************************************************************/ /**************************************************************/ /* THIS SINK MODEL: */ /**************************************************************/ /* tracked_sink_start() * After the vehicle-constants have been filled in, * this computes the rest the situation-dependant constants, * and the initial state. */ STATIC int32 tracked_sink_start( TRACKED_SINK_VARS *sink, float64 z_ocean, float64 z_floor, float64 time, float64 fill_fraction, /* can be zero */ float64 z, /* z, and dzdt are only needed if already sinking */ float64 dzdt, float64 roll, float64 pitch, float64 final_roll, /* if > -7 radians then overrides defaults */ float64 final_pitch) /* if > -7 radians then overrides defaults */ { float64 critical_fill_fraction; float64 t_1, z_1, dzdt_1; float64 mass_max; float64 a, c; /* intermediates in z, dzdt calcs */ sink->z_ocean = z_ocean; sink->z_floor = z_floor += sink->extra_z_floor; sink->t_previous = time; sink->roll_pitch[0] = roll; sink->roll_pitch[0] = pitch; sink->z = z; sink->dzdt = dzdt; if (final_roll > -7) sink->rp_final[0] = final_roll; if (final_pitch > -7) sink->rp_final[1] = final_pitch; /* mass model initial computations */ /* find t_transition (when can no longer float) */ if (fill_fraction > sink->max_fill_fraction) fill_fraction = sink->max_fill_fraction; /* critical transition occurs when * when mass(t) > water_density * ship_volume, * mass(t) = mass0 + water_density * ship_volume * fill_fraction * => critical fill_fraction = 1 - mass0/(water_density * ship_volume) */ sink->mass_water = WATER_DENSITY * sink->len[0] * sink->len[1] * sink->len[2]; sink->fill_fraction_critical = 1 - sink->mass0 / (sink->mass_water); sink->t_transition = time + (sink->fill_fraction_critical - fill_fraction) / sink->fill_fraction_rate; /* check for t_final (when hits bottom) before t_transition */ /* fill_fraction(t) = critical_fill_fraction + (t-t_transition)*fill_rate * floating depth(t) = ship_height * fill_fraction(t) * z_ocean - z_floor = depth(t_final) * (If hits bottom before t_transition!!:) * => t_final = t_transition * + (((z_ocean - z_floor)/ship_height) * - critical_fill_fraction) * / fill_rate */ sink->t_final /* only if t_final occurs before t_transition */ = sink->t_transition + (((z_ocean - z_floor)/sink->len[2]) - sink->fill_fraction_critical) / sink->fill_fraction_rate; if (sink->t_final <= sink->t_transition) return(1); /* starting ship sinking initial computations */ /* find z_1, dzdt_1, at t_1, which is either now or * at the time of the future transition to sinking */ if (time <= sink->t_transition) { t_1 = sink->t_transition; z_1 = z_ocean - sink->len[2]; dzdt_1 = - sink->len[2] * sink->fill_fraction_rate; } else { t_1 = time; z_1 = sink->z; dzdt_1 = sink->dzdt; } sink->drag_coeff = 2 * WATER_DENSITY * sink->len[0] * sink->len[1] * sink->drag_multiplier; mass_max = sink->mass0 + sink->max_fill_fraction * sink->len[0] * sink->len[1] * sink->len[2] * WATER_DENSITY; /* check for divide by zero */ if (!((sink->drag_coeff <= 0) || (mass_max <= 0))) sink->dzdt_max = sqrt(mass_max * GRAVITY / sink->drag_coeff); else sink->dzdt_max = 10.; sink->t_const = t_1 - (sink->dzdt_max/GRAVITY)*arctanh(-dzdt_1/sink->dzdt_max); a = GRAVITY * (t_1 - sink->t_const) / sink->dzdt_max; c = sink->dzdt_max * sink->dzdt_max / GRAVITY; sink->z_const = z_1 + c * log_cosh( a ); /* t_final from above equation, with z_floor instead of z_1, solved for t*/ sink->t_final = sink->t_const + (sink->dzdt_max/GRAVITY) * inv_log_cosh( (sink->z_const - sink->z_floor) / c) ; return (1); } /* tracked_sink_state() * Compute sink state at a later time */ STATIC int32 tracked_sink_state( TRACKED_SINK_VARS *sink, float64 time, float64 *fill_fraction, float64 *mass, float64 *is_sinking, float64 *z, float64 *dzdt, float64 *roll, float64 *pitch) { int32 irp; /* roll or pitch index */ float64 *rp_target/*[2]*/, rp_t_target; float64 rp_rate[2]; /* time derivative, used in linear interp */ float64 rp[2]; float64 a, c; /* intermediates in z, dzdt calcs */ float64 t_want; /* mass model */ *fill_fraction = sink->fill_fraction_critical + (time - sink->t_transition) * sink->fill_fraction_rate; if (*fill_fraction < 0) *fill_fraction = 0; if (*fill_fraction > sink->max_fill_fraction) *fill_fraction = sink->max_fill_fraction; *mass = sink->mass0 + *fill_fraction * sink->mass_water; *is_sinking = (time >= sink->t_transition); /* z(t) model */ if (time >= sink->t_final) { /* final state */ *z = sink->z_floor; *dzdt = 0; } else if (time <= sink->t_transition) { /* z(t) is given by fill fraction */ *z = sink->z_ocean - sink->len[2] * (*fill_fraction); *dzdt = - sink->len[2] * sink->fill_fraction_rate; } else { if (sink->t_previous < sink->t_transition) { /* update to transtion time first */ tracked_sink_state(sink, sink->t_transition, fill_fraction, mass, is_sinking, z, dzdt, roll, pitch); } a = GRAVITY * (time - sink->t_const) / sink->dzdt_max; c = sink->dzdt_max * sink->dzdt_max / GRAVITY; *dzdt = - sink->dzdt_max * tanh( a ); *z = sink->z_const - c * log_cosh( a ); } /* roll pitch model */ /* linear interp for roll, pitch */ if ((time < sink->t_final)||(sink->t_previous < sink->t_final)) { t_want = (time < sink->t_final)?(time):(sink->t_final); if ((t_want < sink->t_transition) && (sink->t_transition < sink->t_final)) { rp_target = &(sink->rp_transition[0]); rp_t_target = sink->t_transition; } else { rp_target = &(sink->rp_final[0]); rp_t_target = sink->t_final; } for (irp=0; irp<2; irp++) { rp_rate[irp] = (rp_target[irp] - sink->roll_pitch[irp]) /(rp_t_target - sink->t_previous); /* check that rate is not to fast */ if (fabs(rp_rate[irp]) > sink->rp_max_rate[irp]) rp_rate[irp] = sink->rp_max_rate[irp] *((rp_rate[irp]<0)?(-1.):(1.)); rp[irp] = sink->roll_pitch[irp] + rp_rate[irp]*(t_want - sink->t_previous); } *roll = rp[0]; *pitch = rp[1]; } /* save current state for use in next computation */ sink->t_previous = time; sink->z = (*z += sink->z_offset); sink->dzdt = *dzdt; sink->roll_pitch[0] = *roll; sink->roll_pitch[1] = *pitch; return (1); } static float64 arctanh(float64 y) { float64 coshx, xdiff, x; if (y <= -1.) return(-30.); if (y >= 1.) return( 30.); /* Newton-Raphson iteration to solve y=tanh(x) for x * dy/dx = 1/cosh(x)**2 * x_new = x_old + (y - y_old)/(dy/dx) */ x = 0; /* max iter count tends to be about 33, more typically 6 */ do { coshx = cosh(x); x += ( xdiff = ((y - tanh(x)) * coshx * coshx ) ); } while (fabs(xdiff) > 1.e-10) ; return(x); } #ifndef LOG2 #define LOG2 0.69314718055994529 #endif static float64 log_cosh(float64 x) { if (x > 20) return(x - LOG2); else return( log( cosh( x ) ) ); } static float64 inv_log_cosh(float64 y) { float64 x, xdiff; if (y <= 1.e-10) return(0.); x = y + LOG2; do { if (x < 1.e-10) return(0.); x += ( xdiff = ( (y - log_cosh(x)) / tanh(x) ) ); } while (fabs(xdiff) > 1.e-10); return(x); } /**************************************************************/ /**************************************************************/ /* SINK MODEL TEST PROGRAM: */ /**************************************************************/ #ifdef TRACKED_SINK_TEST /* test program for testing this sink model */ #include int32 usage(int32 iarg, char *argv[]) { if (iarg >= 1) fprintf(stderr,"tracked_sink_test bad argument argv[%d] = \"%s\"\n", iarg, argv[iarg]); #define PF(S) fprintf(stderr, S) PF("Usage: testsink \\\n"); #ifndef NO_READER PF(" vehicle_name \\\n"); #else /*NO_READER*/ PF(" ship_width length height mass0 \\\n"); PF(" max_fill_fraction fill_fraction_rate drag_multiplier \\\n"); PF(" roll_trans roll_final roll_max_rate \\\n"); PF(" pitch_trans pitch_final pitch_max_rate \\\n"); #endif /*not NO_READER*/ PF(" z_floor start_fill_fraction start_z start_dzdt start_roll start_pitch \\\n"); PF(" t_max, delta_t\n"); return(0); } int32 main(int32 argc, char *argv[]) { TRACKED_SINK_VARS sink; #ifndef NO_READER READER_UNION veh_name_ru, *veh; #endif int32 iarg = 0; float64 time, t_max, delta_t; float64 fill_fraction, mass, is_sinking, z, dzdt, roll, pitch; #ifdef NO_READER if ( (argc < 22) ||(1!=sscanf(argv[++iarg],"%lf", &sink.len[0])) ||(1!=sscanf(argv[++iarg],"%lf", &sink.len[1])) ||(1!=sscanf(argv[++iarg],"%lf", &sink.len[2])) ||(1!=sscanf(argv[++iarg],"%lf", &sink.mass0)) ||(1!=sscanf(argv[++iarg],"%lf", &sink.max_fill_fraction)) ||(1!=sscanf(argv[++iarg],"%lf", &sink.fill_fraction_rate)) ||(1!=sscanf(argv[++iarg],"%lf", &sink.drag_multiplier)) ||(1!=sscanf(argv[++iarg],"%lf", &sink.rp_transition[0])) ||(1!=sscanf(argv[++iarg],"%lf", &sink.rp_final[0])) ||(1!=sscanf(argv[++iarg],"%lf", &sink.rp_max_rate[0])) ||(1!=sscanf(argv[++iarg],"%lf", &sink.rp_transition[1])) ||(1!=sscanf(argv[++iarg],"%lf", &sink.rp_final[1])) ||(1!=sscanf(argv[++iarg],"%lf", &sink.rp_max_rate[1])) #else if ( (argc < 10) ||(!(veh_name_ru.charptr = argv[++iarg])) #endif /*NO_READER*/ ||(1!=sscanf(argv[++iarg],"%lf", &sink.z_floor)) ||(1!=sscanf(argv[++iarg],"%lf", &fill_fraction)) ||(1!=sscanf(argv[++iarg],"%lf", &sink.z)) ||(1!=sscanf(argv[++iarg],"%lf", &sink.dzdt)) ||(1!=sscanf(argv[++iarg],"%lf", &roll)) ||(1!=sscanf(argv[++iarg],"%lf", &pitch)) ||(1!=sscanf(argv[++iarg],"%lf", &t_max)) ||(1!=sscanf(argv[++iarg],"%lf", &delta_t)) ) return(usage(iarg, argv)); #ifndef NO_READER trk_sea_veh_ru_read(NULL, READER_DEFAULTS); tracked_sink_get_veh_invalid(&sink); if (0!=(veh=trk_sea_get_veh_ru("name", READER_CHARPTR, veh_name_ru))) tracked_sink_get_veh(veh, &sink); else return(usage(1,argv)); tracked_sink_get_veh_defaults(&sink); #endif /*not NO_READER*/ tracked_sink_start(&sink, /*z_ocean*/ 0., sink.z_floor, /*time*/ 0., fill_fraction, sink.z, sink.dzdt, roll, pitch, -7., -7.); printf(" t fract mass sinking z dzdt roll pitch\n"); for (time = 0; time <= t_max; time += delta_t) { tracked_sink_state(&sink, time, &fill_fraction, &mass, &is_sinking, &z, &dzdt, &roll, &pitch); printf("%7.3lf %0.4lf %9.4lg %1d %8.3lf %9.3lf %6.3lf %6.3lf\n", time, fill_fraction, mass, (int)(is_sinking), z, dzdt, roll, pitch); } return(0); } #ifndef NO_READER int32 yywrap(void) { return(1); } #endif /*not NO_READER*/ #endif /*TRACKED_SINK_TEST*/ /**************************************************************/