/**************************************************************************** * File: trk_rao.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: Response Amplitude Operators for ocean wave interaction * Created: Sun Sep 1 18:07:38 EDT 1996 * Author: skukolic * $Revision$ * * Remarks: * * * ****************************************************************************/ /* For organizational purposes, this file is split into 4 sections: */ /* INCLUDES AND DECLARATIONS: */ /* READER FILE STUFF */ /* MODSAF INTERFACE */ /* THIS RAO MODEL: */ /* RAO MODEL TEST PROGRAM: */ /**************************************************************/ /**************************************************************/ /* INCLUDES AND DECLARATIONS: */ /**************************************************************/ #include #ifdef TRK_RAO_TEST #define TRACKED_RAO_TEST #endif /*TRK_RAO_TEST*/ #ifdef TRACKED_RAO_TEST #ifndef NO_MODSAF #define NO_MODSAF #endif /*not NO_MODSAF*/ #endif /*TRACKED_RAO_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 /* for EnvXYZ coords */ #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 RAO model defines, and structs */ /* physical constants: */ #define WATER_DENSITY 1000.0 /* kg/m^3 */ #define GRAVITY 9.8 /* m/s^2 */ #define DEFAULT_RAO_DAMP_CONSTANT (0.3) /* Hull directions for which we will calculate ship's amplitude and phase * response to each input wave frequency. */ typedef enum { RAO_Z, RAO_PITCH, /* angle about X axis, X points right out of ship */ RAO_ROLL, /* angle about Y axis, Y axis points out of front of ship */ N_RAO_DIM } RAO_DIM; typedef struct { float64 w0, damp; /* damp is very roughly the amplitude lost fraction per cycle, * friction coeff b = damp*w0/Pi, this properly defines damp * pole_response(w) = 1/((-I*w)**2 + (-I*w)*b + w0**2) * = (w0**2 - w**2 + I*w*b) * /((w0**2 - w**2)**2 + (w*b)**2). */ } RAO_POLE; typedef struct { /* dims */ float64 mass, len[3]; /* kg, meters */ float64 waterline; /* height from ship bottom */ /* poles */ RAO_POLE rao_poles[N_RAO_DIM]; /* for getting amplitude and phase * response to driving frequency */ /* inertias */ float64 inertias[N_RAO_DIM]; /* mass, and moments of inertia */ /* pos, vel */ float64 pos[3], vel[3], y_direction[2]; /* inputs describing ship state*/ } RAO_VEH; /**************************************************************/ /* utility definitions used by this RAO model */ /* for holding and calculating wave spectra: */ typedef struct { float32 frequency; float32 amplitude; float32 phase; float32 wave_number[2]; } SEA_SPECTRAL_DATA_REC; /* for use with a function which iterates over the wave spectra: */ typedef struct { float64 pos0[4]; int32 nwaves, nfreqs, iwave, ifreq; } SPECTRA_ITER; /* COMPLEX number arithmetic needed in RAO calculations */ typedef struct { float64 re, im; } COMPLEX; #define complex_real_part(C) ((C).re) #define complex_imag_part(C) ((C).im) #define complex_from_cart(RE,IM,R) \ (((R)->re = (RE)), ((R)->im = (IM))) #define complex_from_polar(A,P,R) \ (((R)->re = (A)*cos(P)), ((R)->im = (A)*sin(P))) #define complex_add(A,B,R) \ (((R)->re = (A).re + (B).re), ((R)->im = (A).im + (B).im)) #define complex_mult(A,B,R) \ (((R)->re = (A).re * (B).re - (A).im * (B).im), \ ((R)->im = (A).re * (B).im + (A).im * (B).re)) /**************************************************************/ /* RAO 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); /* Get a single field value. */ STATIC float64 trk_sea_get_veh_float( READER_UNION *veh, char *tagname); /* Fill in the initial rao fields from the vehicle description, from * the reader file */ STATIC int32 tracked_rao_get_veh( READER_UNION *veh, RAO_VEH *rao); #endif /*NO_READER*/ #ifndef NO_MODSAF #endif /*not NO_MODSAF*/ /* Basic RAO functions */ #define TRK_SEA_FIELD_INVALID (-98765.) #define SET_IF_NOT_SET(VAR, VALUE) \ if (TRK_SEA_FIELD_INVALID == (VAR)) (VAR) = (VALUE) /* Fill in invalid values, to start, so later we will know * which values have not yet been filled in. */ STATIC int32 tracked_rao_get_veh_invalid(RAO_VEH *rao); /* void tracked_rao_generic_check_dims(RAO_VEH *rao_veh) * Fill in any of mass,lens or waterline, which are currenly zero.*/ STATIC void tracked_rao_generic_check_dims(RAO_VEH *rao_veh); /* void tracked_rao_generic_get_rao_poles(RAO_VEH *rao_veh) * Fill in rao_poles (i.e. resonant frequency and damping) for * each RAO direction, using brick shape and known mass and lengths.*/ STATIC void tracked_rao_generic_get_rao_poles( RAO_VEH *rao_veh); /* void tracked_rao_generic_get_inertias(RAO_VEH *rao_veh) * For each RAO dim, fill in mass and moments of inertia, * using given ship mass and lengths. */ STATIC void tracked_rao_generic_get_inertias( RAO_VEH *rao_veh); /* void tracked_rao_model(...) * Computes resulting position and absolute angle deflections, for each RAO * direction, at the given time. Requires all of the rao_veh struct * inputs (mass, lens, poles, inertias, pos, vel, y_direction) * to be filled in. The input wave spectra will be used along with * the ship's dimensions and position to compute the periodic forces * on the ship (treating the ship like a brick).*/ STATIC void tracked_rao_model( RAO_VEH *rao_veh, float64 time, SPECTRA_ITER *iter, int32 (*rao_spectra_iter)(SPECTRA_ITER *iter, float64 pos[3], SEA_SPECTRAL_DATA_REC *spectra), float64 results[N_RAO_DIM]); /* Used by tracked_rao_model() */ /* static void tracked_rao_model_get_complex_force(...) * Returns a complex number which describes the periodic * force on the ship for the given RAO dimension and input wavelength. * The actual force can be obtained from the complex number result, * which has the correct amplitude and phase, by multiplying * F_complex by exp(-i*w*t), and taking the real part. */ STATIC void tracked_rao_model_get_complex_force( RAO_DIM d, float64 kp[2], float64 len[2], COMPLEX *force); /* void tracked_rao_model_pole_response(...) * For a given pole (resonant frequency and damp factor), this * function computes the amplitude and phase response (as * a complex number) at the given driving frequency. This factor * includes the ship dynamics portion of RAO, but not the * wave force portion of the RAO. */ STATIC void tracked_rao_model_pole_response( RAO_POLE *pole, float64 omega, COMPLEX *response); /**************************************************************/ /**************************************************************/ /* 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; i= 3)) { if (READER_UTYPE(field_pair, 2)==READER_INTEGER) return((float64)(field_pair[2].integer)); else if (READER_UTYPE(field_pair, 2)==READER_REAL) return((float64)(field_pair[2].real)); } return(TRK_SEA_FIELD_INVALID); } /* TRACKED_RAO SPECIFIC: */ /* Table for parsing reader file description of a vehicle into * the appropriate RAO_VEH fields. * This table will contain pairs: {reader_symbol_name, rao_vars_offset} */ static struct veh_fields { char *sym; int32 offset; } tracked_rao_veh_fields[] = { /* offsets will be stored as float64 array references */ #define AOFFSET(FIELD) \ (((int32)(&((RAO_VEH*)0)->FIELD))/sizeof(float64)) { "mass", AOFFSET(mass)}, { "width", AOFFSET(len[0])}, { "length", AOFFSET(len[1])}, { "height", AOFFSET(len[2])}, { "z_w0", AOFFSET(rao_poles[RAO_Z].w0)}, { "z_damp", AOFFSET(rao_poles[RAO_Z].damp)}, { "pitch_w0", AOFFSET(rao_poles[RAO_PITCH].w0)}, { "pitch_damp", AOFFSET(rao_poles[RAO_PITCH].damp)}, { "roll_w0", AOFFSET(rao_poles[RAO_ROLL].w0)}, { "roll_damp", AOFFSET(rao_poles[RAO_ROLL].damp)}, { "z_inertia", AOFFSET(inertias[RAO_Z])}, { "pitch_inertia", AOFFSET(inertias[RAO_PITCH])}, { "roll_inertia", AOFFSET(inertias[RAO_ROLL])}, { "waterline", AOFFSET(waterline)}, { 0, 0 } }; static int tracked_rao_veh_fields_inited = 0; /* Fill in the initial rao fields from the vehicle description, from * the reader file */ STATIC int32 tracked_rao_get_veh( READER_UNION *veh, RAO_VEH *rao) { READER_UNION *field; struct veh_fields *fieldsp; float64 *raoa; if (!tracked_rao_veh_fields_inited) { /* convert veh_fields strings into reader symbols */ for (fieldsp = tracked_rao_veh_fields; fieldsp->sym; ++fieldsp) fieldsp->sym = reader_get_symbol(fieldsp->sym); tracked_rao_veh_fields_inited = 1; } raoa = (float64*)(rao); for (fieldsp = tracked_rao_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) { SET_IF_NOT_SET(raoa[fieldsp->offset], field[2].real); } else if (READER_UTYPE(field, 2)== READER_INTEGER) { SET_IF_NOT_SET(raoa[fieldsp->offset], field[2].integer); } } } return(1); } #endif /*not NO_READER*/ /**************************************************************/ /**************************************************************/ /* MODSAF INTERFACE */ /**************************************************************/ #ifndef NO_MODSAF /* local control vars: */ static int32 trk_rao_mode = 2; /* 0=non, 1=simple, 2=full */ static float64 trk_rao_time_delay_compensate = 0.; /* add to time for veh * calc. */ static float64 trk_rao_extra_z = 0.; /* shift vehicle up this extra amount */ static float64 trk_rao_response_attenuate = 0.9; /* fudge multiplier for responses*/ int32 tracked_rao_veh_id_debug = 0; /* id of vehicle to print debug info on */ /* local functions: */ /* Calculate the new roll, pitch and Z-offset of the specified entity, * based on the ocean heights. */ static void tracked_run_rao_simple(int32 vehicle_id, TRACKED_VARS *tracked, ObjectType guise, float64 dt, float32 *roll, float32 *pitch, float64 posit[3], float64 dir[3]); /* Calculate the new roll, pitch and Z-offset of the specified entity, * based on its RAOs. */ static void tracked_run_rao_full(int32 vehicle_id, TRACKED_VARS *tracked, SPECTRA_ITER *iter, ObjectType guise, float64 dt, float32 *roll, float32 *pitch, float64 posit[3], float64 dir[3], float64 vel[3]); static int32 tracked_rao_get_veh_physdb_dims( int32 vehicle_id, RAO_VEH *rao_veh); /* to be used in a loop looking like: * for (tracked_rao_wave_spectra_iter(iter,pos,0); * tracked_rao_wave_spectra_iter(iter,pos,&spectra); ) * ... */ static int32 tracked_rao_wave_spectra_iter( SPECTRA_ITER *iter, float64 pos[3], SEA_SPECTRAL_DATA_REC *spectra); #ifdef OLD_FLAT static void process_reader_file(char *data_path, char *filename); #endif /**************************************************************/ /* ModSAF interface */ #ifdef USE_PARSER #include #include #include static void trk_rao_parser_init(void); #endif void tracked_init_rao(char *data_path) { /*if (!trk_sea_veh_ru_read(data_path, READER_DEFAULTS)) return(0);*/ trk_sea_veh_ru_read(data_path, READER_DEFAULTS); #ifdef USE_PARSER trk_rao_parser_init(); #else ; #endif } int32 tracked_have_rao(ObjectType guise) { #ifndef OLD READER_UNION guise_ru, *veh_ru; float64 val; guise_ru.integer = guise; if (0!=(veh_ru = trk_sea_get_veh_ru("guise", READER_INTEGER, guise_ru))) { val = trk_sea_get_veh_float(veh_ru, "no_wave_response"); if ((val==0)||(val==TRK_SEA_FIELD_INVALID)) return(TRUE); } return(FALSE); #else switch (guise) { case vehicle_USMC_AAV: case vehicle_USMC_AAV_C7: case vehicle_USMC_AAV_MINE: case vehicle_USMC_AAV_REC: return TRUE; case vehicle_US_DD963: return TRUE; default: return FALSE; } #endif } /* Computes new absolute angles and z position, * using time synchronized to the wave. The full model * uses a steady state model, and the correct x y position * to find the absolute z position and angle deviations. */ void tracked_run_rao(int32 vehicle_id, TRACKED_VARS *tracked, ObjectType guise, float64 dt, float32 *roll, float32 *pitch, float64 posit[4], float64 dir[3], float64 vel[3]) { SPECTRA_ITER iter; float32 pos32[4], vel32[4], out32[4]; float64 lposit[3], ldir[3], lvel[3]; /* in EnvXYZ coordinates */ ecloud_envxyz *envxyz = 0; CTDB *ctdb = gcs_get_tdb((int32)posit[CELL3D], FALSE); CTDB_ELEV_DATA elev[CTDB_MAX_ELEVS]; float64 water_z; int32 eidx; /* convert positions etc. to EnvXYZ coords */ VMAT4_RECAST_VEC(posit, pos32); ecloud_envxyz_gcs_to_envxyz32(envxyz, pos32, out32, NULL); VMAT3_RECAST_VEC(out32, lposit); VMAT3_RECAST_VEC(vel, vel32); vel32[CELL3D] = pos32[CELL3D]; ecloud_envxyz_gcs_to_envxyz32(envxyz, vel32, out32, pos32); VMAT3_RECAST_VEC(out32, lvel); VMAT3_RECAST_VEC(dir, vel32); vel32[CELL3D] = pos32[CELL3D]; ecloud_envxyz_gcs_to_envxyz32(envxyz, vel32, out32, pos32); VMAT3_RECAST_VEC(out32, ldir); VMAT4_RECAST_VEC(posit, iter.pos0); /* save GCS position */ /* do model */ if (trk_rao_mode==1) tracked_run_rao_simple(vehicle_id, tracked, guise, dt, roll, pitch, posit, dir); else if (trk_rao_mode==2) tracked_run_rao_full(vehicle_id, tracked, &iter, guise, dt, roll, pitch, posit, dir, vel); else { *roll = 0; *pitch = 0; posit[2] = 0; } /* Find the Z-value of the water surface. With the transition to * multi-cell GCS databases, this is not usually (if ever) zero, * and is typically negative, yielding "flying AAV's". * */ ctdb_lookup_elevation_mes(ctdb, posit[X], posit[Y], CTDB_LAND_AND_WATER, elev); water_z = 0.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_WATER) water_z = elev[eidx].z; } posit[2] += trk_rao_extra_z; posit[Z] += water_z; /* Adjust for elevation of water surface. */ if (vehicle_id == tracked_rao_veh_id_debug) printf("veh %d: t= %lg, z_off= %lg, pitch= %g, roll= %g\n", vehicle_id, dt, posit[2], *pitch, *roll); } /**************************************************************/ #ifdef USE_PARSER static void trk_rao_mode_set( int32 argc, int32 argv[]) { trk_rao_mode = argv[0]; } static void trk_rao_time_delay_compensate_set( int32 argc, float32 argv[]) { trk_rao_time_delay_compensate = argv[0]; } static void trk_rao_extra_z_set( int32 argc, float32 argv[]) { trk_rao_extra_z = argv[0]; } static void trk_rao_response_attenuate_set( int32 argc, float32 argv[]) { trk_rao_response_attenuate = argv[0]; } static void trk_rao_veh_id_debug_set( int32 argc, int32 argv[]) { tracked_rao_veh_id_debug = argv[0]; } static void trk_rao_vars_print( int32 argc, float32 argv[]) { static char *modes[3] = {"non", "simple", "full"}; printf(" trk_rao_mode = %s\n", modes[ ((0 <= trk_rao_mode) && (trk_rao_mode < 3)) ?(trk_rao_mode):(0)]); printf("trk_rao_time_delay_compensate = %lg\n", trk_rao_time_delay_compensate); printf(" trk_rao_extra_z = %lg\n", trk_rao_extra_z); printf(" trk_rao_response_attenuate = %lg\n", trk_rao_response_attenuate); printf(" trk_rao_veh_id_debug = %ld\n", tracked_rao_veh_id_debug); } #ifndef lint /* libparser command tables, for the command being added the the * text user interface. */ static CONSTANT_TABLE(trk_rao_modes_table) CONSTANT("non", 0, "- do no RAO calculations") CONSTANT("simple", 1, "- get orientation from sea heights") CONSTANT("full", 2, "- full RAO calculations") END_CONSTANT_TABLE static DEFINE_TABLE(trk_rao_local_command_table) KEYWORD_SELECT(" tracked ship RAO commands (libtracked) ") KEYWORD ("print", "- print RAO calculation mode etc") CALL (trk_rao_vars_print) END_KEYWORD KEYWORD ("mode", "- RAO calculation mode: non or simple or full") GETCONSTANT(trk_rao_modes_table) CALL (trk_rao_mode_set) END_KEYWORD KEYWORD ("time_delay_compensate", "- produce ship positions ahead of time") GETFLOAT("time ahead (seconds)") CALL (trk_rao_time_delay_compensate_set) END_KEYWORD KEYWORD ("extra_z", "- extra ship elevation, for every ship") GETFLOAT("z") CALL (trk_rao_extra_z_set) END_KEYWORD KEYWORD ("attenuate_response", "- extra ship response attenuation (default 1.0)") GETFLOAT("attenuation_factor") CALL (trk_rao_response_attenuate_set) END_KEYWORD KEYWORD ("veh_id_debug", "- vehicle id for printing debug info on") GETDECIMAL("vehicle_id") CALL (trk_rao_veh_id_debug_set) END_KEYWORD END_KEYWORD_SELECT END_DEFINE_TABLE static DEFINE_TABLE(trk_rao_command_table) KEYWORD ("trk_rao", "- tracked ship RAO commands (libtracked)") DO_KEYWORD_TABLE(trk_rao_local_command_table) END_KEYWORD END_DEFINE_TABLE #endif /* lint */ static void trk_rao_parser_init(void) { ParseAddTable(eui_command_table, trk_rao_command_table); } #endif /* USE_PARSER */ /**************************************************************/ /* locked to ocean RAO model */ /* * read in the tracked data file... * the rdr file should be in this format: * * ( * (max_speed * (AVLB 4.0) * (on_bridge 4.0) * (in_breach_lane 4.0) * ) * ) * */ #if 0 static void process_reader_file(char *data_path, char *filename) READER_UNION ru, *field, *speeds; if (reader_read(filename, data_path, &ru, READER_DEFAULTS) != READER_READ_OK) { fprintf(stderr, "\nCouldn't find %s.. using defaults\n", filename); return; } if ((tracked_speeds = reader_find_tag(reader_get_symbol("max_speed"), ru.array, READER_UNTAGGED, 0)) == NULL) { fprintf(stderr, "\nProblem reading %s..\n", filename); return; } } #endif /* * Calculate the new roll, pitch and Z-offset of the specified entity, * based on the ocean heights. */ static void tracked_run_rao_simple(int32 vehicle_id, TRACKED_VARS *tracked, ObjectType guise, float64 dt, float32 *roll, float32 *pitch, float64 posit[3], float64 dir[3]) { float32 alt_roll, alt_pitch; ENV_INTERFACE ei; /* environmental interface */ uint32 num_waves; uint32 num_frequencies; float64 tide_level, my_length, my_beam, new_z /*,dir[3]*/; float64 ctr_height, bow_height, stern_height, port_height, stbd_height; int32 wavei, freqi; static float32 pitch_limit = 30.0, roll_limit = 30.0; new_z = posit[Z]; /* * $$$ Hard-coded Z adjustment: Spruance has draft of 29 feet. * AAV has draft (eyeballed) of 4 feet. */ switch (guise) { case vehicle_USMC_AAV: case vehicle_USMC_AAV_C7: case vehicle_USMC_AAV_MINE: case vehicle_USMC_AAV_REC: #if 0 new_z -= FT_TO_M(3.9); #endif my_length = 8.2; my_beam = 3.3; break; case vehicle_US_DD963: default: #if 0 new_z -= FT_TO_M(29.0) #endif my_length = FT_TO_M(563.0); my_beam = FT_TO_M(55.0); break; #if 0 default: return; #endif } #define FAKE_OUT_LIBENVSEA_TIME #ifdef FAKE_OUT_LIBENVSEA_TIME /* temporarily change envsea_base_time for env_gets on sea height */ envsea_base_time -= trk_rao_time_delay_compensate; #endif /* * Get the sea height at the bow, stern, starboard and port edges. * Hard-coded offsets represent the dimensions of an AAV. This * has the side effect of exercising the underlying P-M model, * which has an expense we'd like to assess. */ bzero(&ei, sizeof(ENV_INTERFACE)); ei.u.sea_surface_height.input_vector[X] = posit[X]; ei.u.sea_surface_height.input_vector[Y] = posit[Y]; ei.u.sea_surface_height.input_vector[Z] = posit[Z]; ei.u.sea_surface_height.input_vector[CELL3D] = posit[CELL3D]; env_get(ENV_SEA_SURFACE_HEIGHT, &ei); ctr_height = ei.u.sea_surface_height.output_value; new_z += ctr_height; /*ent_get_direction(vehicle_id, dir);*/ bzero(&ei, sizeof(ENV_INTERFACE)); ei.u.sea_surface_height.input_vector[X] = posit[X] + dir[1] * (my_length/2.0); ei.u.sea_surface_height.input_vector[Y] = posit[Y] + dir[0] * (my_length / 2.0); ei.u.sea_surface_height.input_vector[Z] = posit[Z]; env_get(ENV_SEA_SURFACE_HEIGHT, &ei); bow_height = ei.u.sea_surface_height.output_value; bzero(&ei, sizeof(ENV_INTERFACE)); ei.u.sea_surface_height.input_vector[X] = posit[X] - dir[1] * (my_length/2.0); ei.u.sea_surface_height.input_vector[Y] = posit[Y] - dir[0] * (my_length / 2.0); ei.u.sea_surface_height.input_vector[Z] = posit[Z]; ei.u.sea_surface_height.input_vector[CELL3D] = posit[CELL3D]; env_get(ENV_SEA_SURFACE_HEIGHT, &ei); stern_height = ei.u.sea_surface_height.output_value; bzero(&ei, sizeof(ENV_INTERFACE)); ei.u.sea_surface_height.input_vector[X] = posit[X] + dir[0] * (my_beam / 2.0); ei.u.sea_surface_height.input_vector[Y] = posit[Y] + dir[1] * (my_beam / 2.0); ei.u.sea_surface_height.input_vector[Z] = posit[Z]; ei.u.sea_surface_height.input_vector[CELL3D] = posit[CELL3D]; env_get(ENV_SEA_SURFACE_HEIGHT, &ei); stbd_height = ei.u.sea_surface_height.output_value; bzero(&ei, sizeof(ENV_INTERFACE)); ei.u.sea_surface_height.input_vector[X] = posit[X] - dir[0] * (my_beam / 2.0); ei.u.sea_surface_height.input_vector[Y] = posit[Y] - dir[1] * (my_beam / 2.0); ei.u.sea_surface_height.input_vector[Z] = posit[Z]; ei.u.sea_surface_height.input_vector[CELL3D] = posit[CELL3D]; env_get(ENV_SEA_SURFACE_HEIGHT, &ei); port_height = ei.u.sea_surface_height.output_value; #ifdef FAKE_OUT_LIBENVSEA_TIME /* put back */ envsea_base_time += trk_rao_time_delay_compensate; #endif alt_roll = asin((port_height-stbd_height)/my_beam); alt_pitch = asin((stern_height-bow_height)/my_length); /* * Protect ourselves from impossible/inconvenient roll/pitch * values by clipping to +/- 30 degrees. */ if (alt_roll > DEG_TO_RAD(roll_limit)) { #if 0 DEBUG_TRACKED("Roll clipped from %.1f\n", RAD_TO_DEG(alt_roll)); #endif alt_roll = DEG_TO_RAD(roll_limit); } else if (alt_roll < DEG_TO_RAD(-roll_limit)) { #if 0 DEBUG_TRACKED("Roll clipped from %.1f\n", RAD_TO_DEG(alt_roll)); #endif alt_roll = DEG_TO_RAD(-roll_limit); } if (alt_pitch > DEG_TO_RAD(pitch_limit)) { #if 0 DEBUG_TRACKED("Pitch clipped from %.1f\n", RAD_TO_DEG(alt_pitch)); #endif alt_pitch = DEG_TO_RAD(pitch_limit); } else if (alt_pitch < DEG_TO_RAD(-pitch_limit)) { #if 0 DEBUG_TRACKED("Pitch clipped from %.1f\n", RAD_TO_DEG(alt_pitch)); #endif alt_pitch = DEG_TO_RAD(-pitch_limit); } *roll = alt_roll; *pitch = alt_pitch; posit[Z] = new_z; } /**************************************************************/ /* full RAO model. */ #define CHECK_ANGLE_LIMIT(VAR,LIM) \ if (fabs((VAR))>(LIM)) (VAR)=(LIM)*(((VAR)<0)?(-1.):(1)) #define TRACKED_RAO_ANGLE_LIMIT ((40./180.)*PI) /* Calculate the new roll, pitch and Z-offset of the specified entity, * based on its RAOs. */ static void tracked_run_rao_full(int32 vehicle_id, TRACKED_VARS *tracked, SPECTRA_ITER *iter, ObjectType guise, float64 dt, float32 *roll, float32 *pitch, float64 posit[3], float64 dir[3], float64 vel[3]) { RAO_VEH rao_veh; /* the struct which is incrementally filled in * for this RAO model. */ float64 results[N_RAO_DIM]; ENV_INTERFACE ei; float64 tide_level; struct timeval time_ptr; READER_UNION guise_ru, *veh_ru; /* get tide_level */ bzero(&ei, sizeof(ei.u.sea_tide_level.input_vector)); bcopy(posit, &ei.u.sea_tide_level.input_vector, 3*sizeof(float64)); tide_level = 0.; if (env_get(ENV_SEA_TIDE_LEVEL, &ei) != -1) tide_level += ei.u.sea_tide_level.output_value; #define TRK_RAO_SEA_TIME #ifdef TRK_RAO_SEA_TIME gettimeofday(&time_ptr, (struct timezone *)NULL); dt = (time_ptr.tv_sec + time_ptr.tv_usec * .000001)- envsea_base_time; dt += trk_rao_time_delay_compensate; #endif /* fill rao_veh */ memset(&rao_veh, 0, sizeof(RAO_VEH)); /* first fill with invalid values so that the first function to set * a field is the only function to set that field */ tracked_rao_get_veh_invalid(&rao_veh); guise_ru.integer = ent_get_guise(vehicle_id, 0); if (0!=(veh_ru = trk_sea_get_veh_ru("guise", READER_INTEGER, guise_ru))) tracked_rao_get_veh(veh_ru, &rao_veh); /*from reader file entry*/ tracked_rao_get_veh_physdb_dims(vehicle_id, &rao_veh); /* from physdb */ /* compute any entries not already filled in: */ tracked_rao_generic_check_dims(&rao_veh); tracked_rao_generic_get_rao_poles(&rao_veh); tracked_rao_generic_get_inertias(&rao_veh); /* copy pos, vel */ memcpy(&rao_veh.pos, posit, 3*sizeof(float64)); memcpy(&rao_veh.vel, vel, 3*sizeof(float64)); vmat2_unit64(dir, rao_veh.y_direction); /* do the real calculation: */ tracked_rao_model(&rao_veh, dt, iter, tracked_rao_wave_spectra_iter, results); posit[2] = results[RAO_Z] /*height deviation*/ * trk_rao_response_attenuate + tide_level - rao_veh.waterline; /* bottom height minus * zero height sea level */ CHECK_ANGLE_LIMIT(results[RAO_PITCH], TRACKED_RAO_ANGLE_LIMIT); CHECK_ANGLE_LIMIT(results[RAO_ROLL], TRACKED_RAO_ANGLE_LIMIT); *pitch = results[RAO_PITCH] * trk_rao_response_attenuate; *roll = results[RAO_ROLL] * trk_rao_response_attenuate; /*#define TRK_DEBUG2*/ #ifdef TRK_DEBUG2 printf("veh %d: t= %lg, z_off= %lg, pitch= %g, roll= %g\n", vehicle_id, dt, posit[2], *pitch, *roll); #endif } #include static int32 tracked_rao_get_veh_physdb_dims( int32 vehicle_id, RAO_VEH *rao_veh) { PHYSDB_DATA *pdb; int32 i; /* first get data from physdb: */ pdb = ent_get_physdb(vehicle_id); SET_IF_NOT_SET(rao_veh->mass, pdb->mass_kg); for (i=0; i<3; i++) SET_IF_NOT_SET(rao_veh->len[i], pdb->dimensions[i]); SET_IF_NOT_SET(rao_veh->waterline, pdb->model_base_adjustment); return(1); } /* to be used in a loop looking like: * for (tracked_rao_wave_spectra_iter(iter,pos,0); * tracked_rao_wave_spectra_iter(iter,pos,&spectra); ) * ... */ static int32 tracked_rao_wave_spectra_iter( SPECTRA_ITER *iter, float64 pos[3], SEA_SPECTRAL_DATA_REC *spectra) { ENV_INTERFACE ei; /* environmental interface */ if (!spectra) { bzero(&ei, sizeof(ENV_INTERFACE)); env_get(ENV_SEA_NUM_WAVES, &ei); iter->nwaves = ei.u.sea_num_waves.output_type; bzero(&ei, sizeof(ENV_INTERFACE)); env_get(ENV_SEA_NUM_FREQUENCIES, &ei); iter->nfreqs = ei.u.sea_num_frequencies.output_type; iter->iwave = -1; iter->ifreq = 0; return(1); } else { if ( (iter->ifreq >= iter->nfreqs) ||( (++(iter->iwave) >= iter->nwaves) &&(++(iter->ifreq) >= iter->nfreqs))) return(0); if (iter->iwave >= iter->nwaves) iter->iwave = 0; bzero(&ei, sizeof(ENV_INTERFACE)); ei.u.spectral_data.long_wave_num = iter->iwave; ei.u.spectral_data.frequency_num = iter->ifreq; VMAT4_RECAST_VEC(iter->pos0, ei.u.spectral_data.input_vector); env_get(ENV_SEA_SPECTRAL_RECORD, &ei); spectra->frequency = ei.u.spectral_data.output_frequency; spectra->amplitude = ei.u.spectral_data.output_amplitude; spectra->phase = ei.u.spectral_data.output_phase; spectra->wave_number[0] = ei.u.spectral_data.output_wave_number_X; spectra->wave_number[1] = ei.u.spectral_data.output_wave_number_Y; return(1); } } /**************************************************************/ #endif /*not NO_MODSAF*/ /**************************************************************/ /**************************************************************/ /* THIS RAO MODEL: */ /**************************************************************/ /* Fill in invalid values, to start, so later we will know * which values have not yet been filled in. */ STATIC int32 tracked_rao_get_veh_invalid(RAO_VEH *rao) { float64 *raoa, *raob; raob = (float64*)(&(rao->pos[0])); for (raoa = (float64*)(&(rao->mass)); raoa < raob; ++raoa) *raoa = TRK_SEA_FIELD_INVALID; return(1); } /* void tracked_rao_generic_check_dims(RAO_VEH *rao_veh) * Fill in any of mass,lens or waterline, which are currenly zero.*/ STATIC void tracked_rao_generic_check_dims( RAO_VEH *rao_veh) { int32 i; float64 temp; /* force to be nonzeros: */ if (rao_veh->mass <= 0) rao_veh->mass = 10000.; for (i=0; i<3; i++) if (rao_veh->len[i] <= 0) rao_veh->len[i] = 10.; if (rao_veh->waterline <= 0) { /* approximate depth in water, treating ship as a uniform * density box: */ temp = WATER_DENSITY /* kg/m^3 */ * rao_veh->len[0] * rao_veh->len[1]; if (temp > 0.) rao_veh->waterline = rao_veh->mass / temp; else rao_veh->waterline = 0; } } /* void tracked_rao_generic_get_rao_poles(RAO_VEH *rao_veh) * Fill in rao_poles (i.e. resonant frequency and damping) for * each RAO direction, using brick shape and known mass and lengths.*/ /* These pole formulae describe the ship dynamics porition of the RAO, * and were derived by treating the ship as a brick, and deriving * the restoring forces on this brick given small position or angle * deviations. The restoring forces were calculated using a local verion * of the Archemedies displaced water principle described below. */ STATIC void tracked_rao_generic_get_rao_poles( RAO_VEH *rao_veh) { RAO_DIM d; float64 *len; if ( (rao_veh->mass == 0) ||(rao_veh->len[0] == 0) ||(rao_veh->len[1] == 0) ||(rao_veh->len[2] == 0)) { for (d=0; drao_poles[d].w0, 1); } else { /* simple formulae */ len = rao_veh->len; SET_IF_NOT_SET(rao_veh->rao_poles[RAO_Z].w0, sqrt( GRAVITY * WATER_DENSITY * len[0] * len[1] / rao_veh->mass )); SET_IF_NOT_SET(rao_veh->rao_poles[RAO_PITCH].w0, /* about X axis */ rao_veh->rao_poles[RAO_Z].w0 / sqrt( 1. + ( (len[2] * len[2]) / (len[1] * len[1]) ) ) ); SET_IF_NOT_SET(rao_veh->rao_poles[RAO_ROLL].w0, /* about Y axis */ rao_veh->rao_poles[RAO_Z].w0 / sqrt( 1. + ( (len[2] * len[2]) / (len[0] * len[0]) ) ) ); } /* fudge damp factors for now: */ for (d=0; drao_poles[d].damp, DEFAULT_RAO_DAMP_CONSTANT); } /* void tracked_rao_generic_get_inertias(RAO_VEH *rao_veh) * For each RAO dim, fill in mass and moments of inertia, * using given ship mass and lengths. */ STATIC void tracked_rao_generic_get_inertias( RAO_VEH *rao_veh) { RAO_DIM d; float64 *len; if ( (rao_veh->mass == 0) ||(rao_veh->len[0] == 0) ||(rao_veh->len[1] == 0) ||(rao_veh->len[2] == 0)) { for (d=0; dinertias[d], 1.); } else { /* simple formulae */ len = rao_veh->len; SET_IF_NOT_SET(rao_veh->inertias[RAO_Z], rao_veh->mass); SET_IF_NOT_SET(rao_veh->inertias[RAO_PITCH], rao_veh->mass * (len[1]*len[1] + len[2]*len[2]) / 12.); SET_IF_NOT_SET(rao_veh->inertias[RAO_ROLL], rao_veh->mass * (len[0]*len[0] + len[2]*len[2]) / 12.); } } /* void tracked_rao_model(...) * Computes resulting position and absolute angle deflections, for each RAO * direction, at the given time. Requires all of the rao_veh struct * inputs (mass, lens, poles, inertias, pos, vel, y_direction) * to be filled in. The input wave spectra will be used along with * the ship's dimensions and position to compute the periodic forces * on the ship (treating the ship like a brick). */ STATIC void tracked_rao_model( RAO_VEH *rao_veh, float64 time, SPECTRA_ITER *iter, int32 (*rao_spectra_iter)(SPECTRA_ITER *iter, float64 pos[3], SEA_SPECTRAL_DATA_REC *spectra), float64 results[N_RAO_DIM]) { SEA_SPECTRAL_DATA_REC spectra; RAO_DIM d; float64 frequency, k[2], kp[2]; /* wave vector in ship frame */ COMPLEX forces[N_RAO_DIM], phase, response[3]; #define DOT2(V1,V2) (((V1)[0]*((V2)[0])) + ((V1)[1]*((V2)[1]))) /* for each rao dim, clear result */ for (d=0; dpos, 0); (*rao_spectra_iter)(iter, rao_veh->pos, &spectra); ) { /* compensate for backwards wave vector in PM model: */ k[0] = spectra.wave_number[0]; k[1] = spectra.wave_number[1]; /* phase shift from center of ship position etc. */ complex_from_polar(1., /* uses normal physics convention k.r - w*t: */ + DOT2(k, rao_veh->pos) - spectra.frequency * time + spectra.phase, /* pm has backwards time */ &phase); /* frequency shift from ship velocity */ frequency = spectra.frequency - DOT2(k, rao_veh->vel); /* wave_vector in ship frame */ kp[1] = DOT2(k, rao_veh->y_direction); kp[0] = k[0] * rao_veh->y_direction[1] - k[1] * rao_veh->y_direction[0]; /* for each rao_dim, add in the frequency component response */ for (d=0; dlen, &forces[d]); tracked_rao_model_pole_response(&(rao_veh->rao_poles[d]), frequency, &response[0]); complex_mult(phase, forces[d], &response[1]); complex_mult(response[1], response[0], &response[2]); results[d] += (spectra.amplitude/rao_veh->inertias[d]) * complex_real_part(response[2]); } } } /* static void tracked_rao_model_get_complex_force(...) * Returns a complex number which describes the periodic * force on the ship for the given RAO dimension and input wavelength. * The actual force can be obtained from the complex number result, * which has the correct amplitude and phase, by multiplying * F_complex by exp(-i*w*t), and taking the real part. */ /* This function uses the Archmedies principle locally at each * point on the under surface of the ship to find the force there: * this local interpretation of Archemedies principle states that * the upwards force on the ship is equal to the weight of the * column of displaced water above that point on the ship (including * wave height). For the purposes of integrating this force, * the ship is treated as a rectangular brick, in a horizontal position. * The effective restoring force was already included when the pole * frequencies were calculated. */ STATIC void tracked_rao_model_get_complex_force( RAO_DIM d, float64 kp[2], float64 len[2], COMPLEX *force) { int32 moment[2], i; float64 kX[2], result; moment[0]=0; moment[1]=0; result = 1.; if (d == RAO_PITCH) moment[1] = 1; if (d == RAO_ROLL) { moment[0] = 1; result *=-1; } for (i=0; i<2; i++) { #define EPS1 1.e-3 kX[i] = 0.5*kp[i]*len[i]; if (moment[i]==0) { if (fabs(kX[i]) > EPS1) result *= (2./kp[i])*sin(kX[i]); else result *= len[i]; } else { if (fabs(kX[i]) > EPS1) result *= (-(len[i]/kp[i])*cos(kX[i]) +(2./(kp[i]*kp[i]))*sin(kX[i])); else result *= - kX[i]*len[i]*len[i]/6.; } } result *= GRAVITY * WATER_DENSITY; if (moment[0] || moment[1]) complex_from_cart(0., result, force); else complex_from_cart(result, 0., force); } /* void tracked_rao_model_pole_response(...) * For a given pole (resonant frequency and damp factor), this * function computes the amplitude and phase response (as * a complex number) at the given driving frequency. This factor * includes the ship dynamics portion of RAO, but not the * wave force portion of the RAO. */ STATIC void tracked_rao_model_pole_response( RAO_POLE *pole, float64 omega, COMPLEX *response) { /* damp is very roughly the decay fraction per cycle, * friction coeff b = damp*w0/Pi, this properly defines damp * pole_response(w) = 1/((-I*w)**2 + (-I*w)*b + w0**2) * = (w0**2 - w**2 + I*w*b) * /((w0**2 - w**2)**2 + (w*b)**2). */ float64 bw, w02minusw2, mag2; bw = pole->damp * omega * (1./PI); w02minusw2 = ( pole->w0 * pole->w0 ) - ( omega * omega ); if (0 != (mag2 = w02minusw2*w02minusw2 + bw*bw)) complex_from_cart(w02minusw2/mag2, bw/mag2, response); else complex_from_cart(0., 0., response); } /**************************************************************/ /**************************************************************/ /* RAO MODEL TEST PROGRAM: */ /**************************************************************/ #ifdef TRACKED_RAO_TEST /* test program for testing this RAO model */ #include static int32 tracked_rao_test_spectra_iter( SPECTRA_ITER *iter, float64 pos[3], SEA_SPECTRAL_DATA_REC *spectra); SEA_SPECTRAL_DATA_REC test_spectra = {0, 1, 0, {0, 1}}; int32 usage(int32 iarg, char *argv[]) { if (iarg > 0) fprintf(stderr,"ERROR: bad argument to raotest %d = \"%s\" \n", iarg, argv[iarg]); #ifndef NO_READER printf("Usage: test_trk_rao vehicle_name \\\n"); #else printf("Usage: testrao shipmass(kg) width(m) length(m) height(m) \\\n"); printf(" dampz damppitch damproll \\\n"); #endif printf(" waveheadingfromship(degrees) wave_omega\n"); #ifdef NO_READER printf(" damp factors are roughly fraction per resonance cycle\n"); printf(" e.g. 0.07 to 0.5 \n"); #endif printf(" wave heading is a compass direction from north pointing ship\n"); return(0); } int32 main(int32 argc, char *argv[]) { int32 iarg; RAO_VEH rao_veh; #ifndef NO_READER READER_UNION veh_name_ru, *veh; #endif float64 damp[N_RAO_DIM], waveheadingfromship; float64 wave_number, angle; RAO_DIM d; float64 results_cos[N_RAO_DIM], results_sin[N_RAO_DIM]; float64 results_amp[N_RAO_DIM], results_phase[N_RAO_DIM]; float64 results_amp2[N_RAO_DIM]; static char *rao_dim_names[N_RAO_DIM] = {"Z ","PITCH","ROLL "}; memset(&rao_veh, 0, sizeof(RAO_VEH)); tracked_rao_get_veh_invalid(&rao_veh); iarg = 0; #ifndef NO_READER if ( (argc < 4) ||(!(veh_name_ru.charptr = argv[++iarg])) #else if ( (argc < 10) ||(1>sscanf(argv[++iarg],"%lf", &rao_veh.mass)) ||(1>sscanf(argv[++iarg],"%lf", &rao_veh.len[0])) ||(1>sscanf(argv[++iarg],"%lf", &rao_veh.len[1])) ||(1>sscanf(argv[++iarg],"%lf", &rao_veh.len[2])) ||(1>sscanf(argv[++iarg],"%lf", &damp[RAO_Z])) ||(1>sscanf(argv[++iarg],"%lf", &damp[RAO_PITCH])) ||(1>sscanf(argv[++iarg],"%lf", &damp[RAO_ROLL])) #endif ||(1>sscanf(argv[++iarg],"%lf", &waveheadingfromship)) ||(1>sscanf(argv[++iarg],"%f", &(test_spectra.frequency))) ) return(usage(iarg, argv)); /* fill out wave test_spectra: */ wave_number = test_spectra.frequency * test_spectra.frequency / GRAVITY; if (wave_number > 0) printf("wavelength = %lf\n", 2*PI/wave_number); angle = waveheadingfromship * PI /180.; /* pm wave vectors are backwards: */ test_spectra.wave_number[0] = -wave_number * sin(angle); test_spectra.wave_number[1] = -wave_number * cos(angle); /* fill in rao_veh: */ #ifndef NO_READER trk_sea_veh_ru_read(NULL, READER_DEFAULTS); tracked_rao_get_veh_invalid(&rao_veh); if (0!=(veh=trk_sea_get_veh_ru("name", READER_CHARPTR, veh_name_ru))) { reader_pretty_print(veh, stdout); tracked_rao_get_veh(veh, &rao_veh); } else return(usage(1,argv)); #endif /*not NO_READER*/ tracked_rao_generic_check_dims(&rao_veh); /* fills in nominal values * when 0s*/ tracked_rao_generic_get_rao_poles(&rao_veh); #ifdef NO_READER /* adjust pole damping factors: */ for (d=0; dnwaves = 1; iter->nfreqs = 1; iter->iwave = -1; iter->ifreq = 0; return(1); } else { if ( (iter->ifreq >= iter->nfreqs) ||( (++(iter->iwave) >= iter->nwaves) &&(++(iter->ifreq) >= iter->nfreqs))) return(0); if (iter->iwave >= iter->nwaves) iter->iwave = 0; memcpy(spectra, &test_spectra, sizeof(SEA_SPECTRAL_DATA_REC)); return(1); } } #ifndef NO_READER int32 yywrap(void) { return(1); } #endif /*not NO_READER*/ #endif /*TRACKED_RAO_TEST*/ /**************************************************************/