34#include <libFreeWRL.h>
36#include "../vrml_parser/Structs.h"
37#include "../vrml_parser/CRoutes.h"
38#include "../main/headers.h"
40#include "../world_script/fieldSet.h"
41#include "../x3d_parser/Bindable.h"
43#include "quaternion.h"
45#include "../opengl/Frustum.h"
46#include "../opengl/Material.h"
47#include "../opengl/OpenGL_Utils.h"
48#include "../input/EAIHelpers.h"
51#include "LinearAlgebra.h"
54#include "Component_Geospatial.h"
55#include "Component_DIS.h"
56#include "Component_Grouping.h"
57#include "RenderFuncs.h"
60void add_node_to_broto_context(
struct X3D_Proto *currentContext,
struct X3D_Node *node);
64#include <sys/socket.h>
65#include <netinet/in.h>
201#include "../DIS/DIS.h"
235static int allow_DIS = 0;
236static char* DISaddress = NULL;
237static int DISport = 0;
238static int DISsite = 0;
239static int DISapplication = 0;
244void fwl_set_DISaddress(
char* address) {
245 DISaddress = address;
247void fwl_set_DISport(
int port) {
250void fwl_set_DISsite(
int site) {
253void fwl_set_DISapplication(
int app) {
254 DISapplication = app;
256static int dis_verbose = FALSE;
257void fwl_set_DISverbose(
int verbose) {
258 dis_verbose = verbose;
263int fwl_get_allow_DIS(){
266char* fwl_get_DISaddress() {
269int fwl_get_DISport() {
272int fwl_get_DISsite() {
275int fwl_get_DISapplication() {
276 return DISapplication;
278void fwl_set_allow_DIS(
int allow){
279 allow_DIS = allow ? 1 : 0;
283static int testset = 0;
284void fwl_set_testset(
int iset) {
287int fwl_get_testset() {
368 PDU_ENTITY_STATE = 1,
372 PDU_SERVICE_REQUEST = 5,
373 PDU_RESUPPLY_OFFER = 6,
374 PDU_RESUPPLY_RECEIVED = 7,
375 PDU_RESUPPLY_CANCEL = 8,
376 PDU_REPAIR_COMPLETE = 9,
377 PDU_REPAIR_RESPONSE = 10,
378 PDU_CREATE_ENTITY = 11,
379 PDU_REMOVE_ENTITY = 12,
380 PDU_START_RESUME = 13,
381 PDU_STOP_FREEZE = 14,
382 PDU_ACKNOWLEDGE = 15,
383 PDU_ACTION_REQUEST = 16,
384 PDU_ACTION_RESPONSE = 17,
388 PDU_EVENT_REPORT = 21,
390 PDU_ELECTROMAGNETIC_EMISSION = 23,
392 PDU_TRANSMITTER = 25,
395 PDU_IFF_ATC_NAVAIDS = 28,
396 PDU_UNDERWATER_ACOUSTIC = 29,
397 PDU_SUPPLEMENTAL_EMISSION_ENTITY_STATE = 30,
398 PDU_INTERCOM_SIGNAL = 31,
399 PDU_INTERCOM_CONTROL = 32,
400 PDU_AGGREGATE_STATE = 33,
402 PDU_TRANSFER_CONTROL = 35,
404 PDU_MINEFIELD_STATE =37,
405 PDU_MINEFIELD_QUERY = 38,
406 PDU_MINEFIELD_DATA = 39,
407 PDU_MINEFIELD_RESPONSE_NAK = 40,
408 PDU_ENVIRONMENTAL_PROCESS = 41,
409 PDU_GRIDDED_DATA = 42,
410 PDU_POINT_OBJECT_STATE = 43,
411 PDU_LINEAR_OBJECT_STATE = 44,
412 PDU_AREAL_OBJECT_STATE = 45,
415 PDU_ARTICULATED_PARTS = 48,
417 PDU_LE_DETONATION = 50,
418 PDU_CREATE_ENTITY_R = 51,
419 PDU_REMOVE_ENTITY_R = 52,
420 PDU_START_RESUME_R = 53,
421 PDU_STOP_FREEZE_R = 54,
422 PDU_ACKNOWLEDGE_R = 55,
423 PDU_ACTION_REQUEST_R = 56,
424 PDU_ACTION_RESPONSE_R = 57,
425 PDU_DATA_QUERY_R = 58,
428 PDU_EVENT_REPORT_R = 61,
430 PDU_RECORD_QUERY_R = 63,
431 PDU_SET_RECORD_R = 64,
433 PDU_COLLISION_ELASTIC = 66,
434 PDU_ENTITY_STATE_UPDATE = 67,
435 PDU_ANNOUNCE_OBJECT = 129,
436 PDU_DELETE_OBJECT = 130,
437 PDU_DESCRIBE_APPLICATION = 131,
438 PDU_DESCRIBE_EVENT = 132,
439 PDU_DESCRIBE_OBJECT = 133,
440 PDU_REQUEST_EVENT = 134,
441 PDU_REQUEST_OBJECT = 135,
444void axisangle2ypr(
float *xyza,
float *ypr)
450 float yaw, pitch, roll, x,y,z,a, xyz[3], flen;
451 vecnormalize3f(xyz,xyza);
452 x = xyz[0]; y = xyz[1], z=xyz[2], a=xyza[3];
453 flen = veclength2f(xyz);
456 pitch = acos(-1.0) * .5;
459 pitch = atan(z/flen);
466void ypr2axisangle(
float *ypr,
float *xyza)
472 float yaw, pitch, roll, x,y,z,a, xyz[3];
477 x = cos(pitch)*cos(yaw);
478 y = cos(pitch)*sin(yaw);
483 vecnormalize3f(xyza,xyz);
493float *vector3float2vec3f(
float *b,
struct Vector3Float *a){
505double *vector3double2vec3d(
double *b,
struct Vector3Double *a){
521void TickTime2DISTime(
double ticktime,
int iabs,
unsigned int *hours,
unsigned int *hourfraction ){
522 double hours1970, fraction;
523 unsigned int bitmask;
524 hours1970 = floor(ticktime / 3600.0);
525 *hours = (
unsigned int)hours1970;
526 fraction = (ticktime / 3600.0) - hours1970;
527 *hourfraction = ((
unsigned int)(fraction * pow(2.0,31.0)))<<1;
531 bitmask = bitmask << 1;
532 *hourfraction = *hourfraction & bitmask;
533 if(iabs) *hourfraction |= 1;
535double DISTime2TickTime(
unsigned int hours,
unsigned int hourfraction){
537 double fraction, mantissa;
539 unsigned int bitmask;
541 iabs = (hourfraction & bitmask) != 0 ? TRUE : FALSE;
542 hourfraction = hourfraction >> 1;
543 fraction = hourfraction;
544 fraction /= pow(2.0,31.0);
548 mantissa = floor(TickTime() / 3600.0);
549 mantissa += fraction;
561 struct sockaddr_in saddr;
562 int multicastRelayPort;
563 char *multicastRelayHost;
565 struct Vector *registered;
570void print_stream(
unsigned char *buf,
int nbytes){
572 for(i=0;i<min(210,nbytes);i+=10){
576 printf(
"%5d",(
int)buf[i+j]);
583double *tcs2localswizzled(
double *local,
double *tcs){
589double *local2tcsswizzled(
double *tcs,
double *local){
595void node2pdu_entityType(
int *entityKind,
struct EntityType *entityType){
598 entityType->
entityKind = (
unsigned char) entityKind[0];
599 entityType->
domain = (
unsigned char) entityKind[1];
600 entityType->
country = (
unsigned short) entityKind[2];
601 entityType->
category = (
unsigned char) entityKind[3];
602 entityType->
subcategory = (
unsigned char) entityKind[4];
603 entityType->
specific = (
unsigned char) entityKind[5];
604 entityType->extra = (
unsigned char) entityKind[6];
606void pdu2node_entityType(
struct EntityType *entityType,
int *entityKind){
610 entityKind[1] = entityType->
domain;
611 entityKind[2] = entityType->
country;
612 entityKind[3] = entityType->
category;
614 entityKind[5] = entityType->
specific;
615 entityKind[6] = entityType->extra;
617static int dis_event_number = 0;
618int dis_next_event_number(){
620 return dis_event_number;
622static int dis_fire_mission_index = 0;
623int dis_next_fire_mission_index(){
624 dis_fire_mission_index++;
625 return dis_fire_mission_index;
627struct X3D_Node * dis_find_registered_node_by_entityid(
int entityid,
int sendlist,
int recvlist);
628struct Vector * dis_node2pdus_espdu(
struct X3D_Node *node,
int isHeartbeat){
637 pdus = newVector(
struct Pdu *, 6);
641 if(0) printf(
"es pduchange %d heartbeat %d\n",pnode->_pduchange_es,isHeartbeat);
642 if(pnode->_pduchange_es || isHeartbeat){
653 if(pnode->__geoSystem){
655 Quaternion qgc2tcs, qtcs2body, qgc2body;
656 struct SFVec3d gd, gc, translate;
658 double localxyz[3], tcsxyz[3], tcs2bodyxyz[3], world2bodyxyz[3];
661 gs = GEOSYS(pnode->__geoSystem);
662 user2gc(gs,&pnode->geoCoords,1,&gc);
665 gc2tcsB_transform(gs,&gc,&translate,&rotate);
670 vrmlrot4d_to_quaternion(&qgc2tcs,rotate.c);
671 vrmlrot4f_to_quaternion(&qtcs2body,pnode->rotation.c);
672 quaternion_multiply(&qgc2body,&qgc2tcs,&qtcs2body);
673 quaternion_to_vrmlrot4f(&qgc2body,xyza);
676 axisangle2ypr(xyza,ypr);
686 float2double(tcs.c,pnode->translation.c,3);
688 tcs2gcB(gs,&gc,&tcs,1,&world);
693 if(pnode->deadReckoning < 6){
697 pnode->_change_count++;
698 if(pnode->_change_count > 1){
700 float v1[3], tmp[3], a1[3];
701 dtime = TickTime() - pnode->_lastp0time;
702 vecscale3f(v1,vecdif3f(tmp,pnode->translation.c,pnode->_lastp0.c),1.0f/dtime);
704 if(pnode->_change_count > 2){
706 vecscale3f(a1,vecdif3f(tmp,v1,pnode->linearVelocity.c),1.0f/dtime);
707 veccopy3f(pnode->linearAcceleration.c,a1);
711 vecset3f(pnode->linearAcceleration.c,0.0,0.0,0.0);
713 veccopy3f(pnode->linearVelocity.c,v1);
716 Quaternion qlast,q, qinv, qdif;
717 vrmlrot4f_to_quaternion(&qlast,pnode->_lastr0.c);
718 vrmlrot4f_to_quaternion(&q,pnode->rotation.c);
719 quaternion_inverse(&qinv,&qlast);
720 quaternion_multiply(&qdif,&q,&qinv);
721 quaternion_to_vrmlrot4f(&qdif,pnode->_angularVelocity.c);
722 pnode->_angularVelocity.c[3] *= 1.0f/dtime;
726 veccopy3f(pnode->_lastp0.c,pnode->translation.c);
727 veccopy4f(pnode->_lastr0.c,pnode->rotation.c);
728 pnode->_lastp0time = TickTime();
734 veccopy3f(V,pnode->linearVelocity.c);
735 veccopy3f(A,pnode->linearAcceleration.c);
740 if(pnode->deadReckoning < 6){
744 vrmlrot4d_to_quaternion(&q,rotate.c);
745 quaternion_inverse(&q,&q);
746 quaternion_rotation3f(V,&q,V);
747 quaternion_rotation3f(A,&q,A);
753 vrmlrot4f_to_quaternion(&q,pnode->rotation.c);
754 quaternion_inverse(&q,&q);
755 quaternion_rotation3f(V,&q,V);
756 quaternion_rotation3f(A,&q,A);
769 vecnormalize3f(axis,pnode->_angularVelocity.c);
770 vecscale3f(axis,axis,pnode->_angularVelocity.c[3]);
774 switch(pnode->deadReckoning){
784 if(pnode->__geoSystem){
785 axisangle2ypr(pnode->rotation.c,ypr);
787 vecset3f(ypr,0.0f,0.0f,0.0f);
803 unsigned short iquat16;
805 if(pnode->__geoSystem){
810 Quaternion qtcs, qlocal;
811 struct SFVec3d gc, gd, translate;
814 gs = GEOSYS(pnode->__geoSystem);
815 user2gc(gs,&pnode->geoCoords,1,&gc);
818 gc2tcsB_transform(gs,&gc,&translate,&rotate);
819 vrmlrot4d_to_quaternion(&qtcs,rotate.c);
820 vrmlrot4f_to_quaternion(&qlocal,pnode->rotation.c);
821 quaternion_multiply(&qglobal,&qlocal,&qtcs);
823 vrmlrot_to_quaternion(&qglobal,0.0,1.0,0.0,0.0);
825 quat2double(quat4d,&qglobal);
826 double2float(&quat4f[1],quat4d,3);
827 quat4f[0] = quat4d[3];
829 vecscale4f(quat4f,quat4f,-1.0f);
831 iquat0 = (
unsigned int)(quat4f[0] * 65536);
832 if(quat4f[0] > 65536) iquat0 = 65535;
851 double local2bodyxyz[3], localxyz[3];
852 float2double(local2bodyxyz,pnode->translation.c,3);
853 tcs2localswizzled(localxyz,local2bodyxyz);
874 float *c = pnode->rotation.c;
875 vrmlrot_to_quaternion(&qA,c[0],c[1],c[2],c[3]);
876 quat2euler(ypr,0,&qA);
882 axisangle2ypr(pnode->rotation.c,ypr);
891 pnode->_change_count++;
892 if(pnode->_change_count > 1){
894 float v1[3], tmp[3], a1[3];
895 dtime = TickTime() - pnode->_lastp0time;
896 vecscale3f(v1,vecdif3f(tmp,pnode->translation.c,pnode->_lastp0.c),1.0f/dtime);
898 if(pnode->_change_count > 2){
900 vecscale3f(a1,vecdif3f(tmp,v1,pnode->linearVelocity.c),1.0f/dtime);
901 veccopy3f(pnode->linearAcceleration.c,a1);
905 vecset3f(pnode->linearAcceleration.c,0.0,0.0,0.0);
907 veccopy3f(pnode->linearVelocity.c,v1);
910 Quaternion qlast,q, qinv, qdif;
911 vrmlrot4f_to_quaternion(&qlast,pnode->_lastr0.c);
912 vrmlrot4f_to_quaternion(&q,pnode->rotation.c);
913 quaternion_inverse(&qinv,&qlast);
914 quaternion_multiply(&qdif,&q,&qinv);
915 quaternion_to_vrmlrot4f(&qdif,pnode->_angularVelocity.c);
916 pnode->_angularVelocity.c[3] *= 1.0f/dtime;
919 veccopy3f(pnode->_lastp0.c,pnode->translation.c);
920 veccopy4f(pnode->_lastr0.c,pnode->rotation.c);
921 pnode->_lastp0time = TickTime();
931 vecnormalize3f(axis,pnode->_angularVelocity.c);
932 vecscale3f(axis,axis,pnode->_angularVelocity.c[3]);
936 switch(pnode->deadReckoning){
946 if(pnode->__geoSystem){
947 axisangle2ypr(pnode->rotation.c,ypr);
949 vecset3f(ypr,0.0f,0.0f,0.0f);
965 unsigned short iquat16;
967 if(pnode->__geoSystem){
972 Quaternion qtcs, qlocal;
973 struct SFVec3d gc, gd, translate;
976 gs = GEOSYS(pnode->__geoSystem);
977 user2gc(gs,&pnode->geoCoords,1,&gc);
980 gc2tcsB_transform(gs,&gc,&translate,&rotate);
981 vrmlrot4d_to_quaternion(&qtcs,rotate.c);
982 vrmlrot4f_to_quaternion(&qlocal,pnode->rotation.c);
983 quaternion_multiply(&qglobal,&qlocal,&qtcs);
985 vrmlrot_to_quaternion(&qglobal,0.0,1.0,0.0,0.0);
987 quat2double(quat4d,&qglobal);
988 double2float(&quat4f[1],quat4d,3);
989 quat4f[0] = quat4d[3];
991 vecscale4f(quat4f,quat4f,-1.0f);
993 iquat0 = (
unsigned int)(quat4f[0] * 65536);
994 if(quat4f[0] > 65536) iquat0 = 65535;
1006 pnode->_sent = TRUE;
1008 if(pnode->articulationParameterArray.n){
1010 int i, np = pnode->articulationParameterArray.n;
1015 ap[i].parameterTypeDesignator = 0;
1016 ap[i].parameterType = 1029;
1017 ap[i].parameterValue = pnode->articulationParameterArray.p[i];
1018 ap[i].partAttachedTo = 0;
1023 node2pdu_entityType(&pnode->entityKind,&espdu->
entityType);
1027 vector_pushBack(
struct Pdu*,pdus,(
struct Pdu*)espdu);
1032 if(pnode->_pduchange_fire){
1034 fpdu = (
struct FirePdu *) dis_ctor(type_FirePdu);
1041 if(pnode->fired1 || pnode->fired2){
1042 pnode->firedTime = TickTime();
1064 fpdu->fireMissionIndex = dis_next_fire_mission_index();
1067 float2double(loc,pnode->munitionStartPoint.c,3);
1077 fpdu->
range = pnode->firingRange;
1080 vecdif3f(delta,pnode->munitionEndPoint.c,pnode->munitionStartPoint.c);
1082 vecscale3f(delta,delta,1.0f/3.0f);
1083 vec3f2vector3float(&fpdu->
velocity,delta);
1085 vector_pushBack(
struct Pdu*,pdus,(
struct Pdu*)fpdu);
1088 if(pnode->_pduchange_collision){
1090 cpdu = (
struct CollisionPdu *) dis_ctor(type_CollisionPdu);
1102 vector_pushBack(
struct Pdu*,pdus,(
struct Pdu*)cpdu);
1105 if(pnode->_pduchange_detonation){
1107 dpdu = (
struct DetonationPdu *) dis_ctor(type_DetonationPdu);
1116 pnode->detonateTime = TickTime();
1139 if(pnode->articulationParameterArray.n){
1141 int i, np = pnode->articulationParameterArray.n;
1146 ap[i].parameterTypeDesignator = 0;
1147 ap[i].parameterType = 1029;
1148 ap[i].parameterValue = pnode->articulationParameterArray.p[i];
1149 ap[i].partAttachedTo = 0;
1152 dpdu->articulationParameters = (
void*)ap;
1157 float2double(loc,pnode->detonationLocation.c,3);
1170 vecdif3f(delta,pnode->munitionEndPoint.c,pnode->munitionStartPoint.c);
1172 vecscale3f(delta,delta,1.0f/.5f);
1173 vec3f2vector3float(&dpdu->
velocity,delta);
1177 vector_pushBack(
struct Pdu*,pdus,(
struct Pdu*)dpdu);
1184struct Vector * dis_node2pdus_receiver(
struct X3D_Node *node,
int isHeartbeat){
1187 pdus = newVector(
struct Pdu *, 6);
1190 if(pnode->_pduchange_receiver){
1192 rpdu = (
struct ReceiverPdu *) dis_ctor(type_ReceiverPdu);
1200 rpdu->myRadioCommunicationsFamilyPdu.
radioId = pnode->radioID;
1207 vector_pushBack(
struct Pdu*,pdus,(
struct Pdu*)rpdu);
1211struct Vector * dis_node2pdus_transmitter(
struct X3D_Node *node,
int isHeartbeat){
1214 pdus = newVector(
struct Pdu *, 6);
1218 if(pnode->_pduchange_transmitter){
1247 float2double(loc,pnode->antennaLocation.c,3);
1263 tpdu->
power = pnode->power;
1268 tpdu->
radioEntityType.nomenclature = pnode->radioEntityTypeNomenclature;
1270 tpdu->myRadioCommunicationsFamilyPdu.
radioId = pnode->radioID;
1274 vector_pushBack(
struct Pdu*,pdus,(
struct Pdu*)tpdu);
1278#define ONE_INT32_PER_SIGNAL_DATA_BYTE TRUE
1279struct Vector * dis_node2pdus_signal(
struct X3D_Node *node,
int isHeartbeat){
1282 pdus = newVector(
struct Pdu *, 6);
1286 if(pnode->_pduchange_signal){
1288 spdu = (
struct SignalPdu *) dis_ctor(type_SignalPdu);
1297 spdu->
data = realloc(spdu->
data,pnode->dataLength*
sizeof(
unsigned char));
1298 if(ONE_INT32_PER_SIGNAL_DATA_BYTE){
1300 unsigned char *cdata = (
unsigned char *)spdu->
data;
1302 cdata[k] = (
unsigned char)((
unsigned int)pnode->data.p[k] % 256);
1305 memcpy(spdu->
data,pnode->data.p,pnode->dataLength);
1310 spdu->
samples = pnode->samples;
1311 spdu->
tdlType = pnode->tdlType;
1312 spdu->myRadioCommunicationsFamilyPdu.
radioId = pnode->radioID;
1313 spdu->myRadioCommunicationsFamilyPdu.
entityId.
entity = pnode->entityID;
1314 spdu->myRadioCommunicationsFamilyPdu.
entityId.
site = pnode->siteID;
1316 vector_pushBack(
struct Pdu*,pdus,(
struct Pdu*)spdu);
1325 TAG_SAME_PROGRAM = 1,
1327 TAG_ENTITY_MANAGER = 3,
1329 TAG_TRANSMITTER = 5,
1334struct X3D_Node *dis_find_or_create_espdu_by_category(
int kind,
int domain,
int country,
int category,
int subcategory,
int specific,
int extra);
1335int dis_pdus2node_espdu(
struct X3D_Node *node,
struct Vector *pdus){
1341 if(!pdus)
return ihit;
1342 for(i=0;i<pdus->n;i++)
1344 pdu = vector_get(
struct Pdu*,pdus,i);
1345 if(pdu->
padding == TAG_UNCLAIMED)
1347 case PDU_ENTITY_STATE:
1358 pdu->
padding = TAG_SAME_PROGRAM;
1366 pdu->
padding = avatar ? TAG_AVATAR : TAG_ESPDU;
1368 pnode->timestamp = TickTime();
1369 if(pnode->__geoSystem){
1370 Quaternion qgc2tcs, qtcs2body, qgc2body;
1371 struct SFVec3d gd, gc, translate;
1373 double localxyz[3], tcsxyz[3], tcs2bodyxyz[3], world2bodyxyz[3];
1376 gs = GEOSYS(pnode->__geoSystem);
1377 user2gc(gs,&pnode->geoCoords,1,&gc);
1380 gc2tcsB_transform(gs,&gc,&translate,&rotate);
1391 Quaternion qtcs2gc, q;
1392 float ypr[3], xyza[4];
1396 ypr2axisangle(ypr,xyza);
1400 vrmlrot4f_to_quaternion(&qgc2body,xyza);
1401 vrmlrot4d_to_quaternion(&qgc2tcs,rotate.c);
1402 quaternion_inverse(&qtcs2gc,&qgc2tcs);
1403 quaternion_multiply(&qtcs2body,&qtcs2gc,&qgc2body);
1404 quaternion_set(&q,&qtcs2body);
1405 quaternion_to_vrmlrot4f(&q,pnode->rotation.c);
1411 TRANS_LOCATION_MINUS_GEOCOORD = 2
1414 static int transmethod = TRANS_ZERO;
1417 if(transmethod == TRANS_LOCATION_MINUS_GEOCOORD){
1420 veccopyd(world.c,world2bodyxyz);
1422 gc2tcsB(gs,&gc,&world,1,&tcs);
1423 double2float(pnode->translation.c,tcs.c,3);
1428 struct SFVec3d world, tcs2, tcs1;
1431 static int want_smoothing = 1;
1434 gc2tcsB(gs,&gc,&gc,1,&tcs1);
1435 veccopyd(world.c,world2bodyxyz);
1436 gc2tcs(gs,&gd,&world,1,&tcs2);
1437 gc2tcsB(gs,&gc,&world,1,&tcs2);
1438 vecdifd(deltatcs,tcs1.c,tcs2.c);
1439 double2float(deltap,deltatcs,3);
1440 vecadd3f(pnode->_p0.c,pnode->_p0.c,deltap);
1442 gc2user(gs,&world,1,&pnode->geoCoords);
1444 vecset3f(pnode->translation.c,0.0f,0.0f,0.0f);
1459 if(pnode->deadReckoning < 6){
1463 vrmlrot4d_to_quaternion(&q,rotate.c);
1464 quaternion_rotation3f(V,&q,V);
1465 quaternion_rotation3f(A,&q,A);
1471 vrmlrot4f_to_quaternion(&q,pnode->rotation.c);
1472 quaternion_rotation3f(V,&q,V);
1473 quaternion_rotation3f(A,&q,A);
1479 veccopy3f(pnode->linearAcceleration.c,A);
1480 veccopy3f(pnode->linearVelocity.c,V);
1485 float axis[3], angle;
1487 angle = veclength3f(axis);
1488 vecnormalize3f(pnode->_angularVelocity.c,axis);
1489 pnode->_angularVelocity.c[3] = angle;
1491 if (disverbose() && pdu->
padding == TAG_AVATAR) {
1509 float *c = pnode->rotation.c;
1513 euler2quat(&qA,ypr[0],ypr[1],ypr[2]);
1516 quaternion_to_vrmlrot(&qA,&r[0],&r[1],&r[2],&r[3]);
1527 ypr2axisangle(ypr,pnode->rotation.c);
1535 float axis[3], angle;
1537 angle = veclength3f(axis);
1538 vecnormalize3f(pnode->_angularVelocity.c,axis);
1539 pnode->_angularVelocity.c[3] = angle;
1546 if(pnode->articulationParameterArray.n){
1549 int i, np = pnode->articulationParameterArray.n;
1551 pp = malloc(np *
sizeof(
float));
1556 pp[i] = (float)ap[i].parameterValue;
1560 case 0: pnode->articulationParameterValue0_changed = pp[i];
break;
1561 case 1: pnode->articulationParameterValue1_changed = pp[i];
break;
1562 case 2: pnode->articulationParameterValue2_changed = pp[i];
break;
1563 case 3: pnode->articulationParameterValue3_changed = pp[i];
break;
1564 case 4: pnode->articulationParameterValue4_changed = pp[i];
break;
1565 case 5: pnode->articulationParameterValue5_changed = pp[i];
break;
1566 case 6: pnode->articulationParameterValue6_changed = pp[i];
break;
1567 case 7: pnode->articulationParameterValue7_changed = pp[i];
break;
1572 if(pnode->articulationParameterArray.p) free(pnode->articulationParameterArray.p);
1573 pnode->articulationParameterArray.p = pp;
1576 pdu2node_entityType(&espdu->
entityType,&pnode->entityKind);
1577 pnode->_pduchange_es = TRUE;
1603 pnode->timestamp = TickTime();
1606 pnode->firedTime = TickTime();
1608 pnode->fired1 = TRUE;
1624 if(!muni) printf(
"no muni\n");
1626 printf(
"got muni\n");
1627 pnode->munitionEntityID = muni->entityID;
1628 pnode->munitionSiteID = muni->siteID;
1629 pnode->munitionApplicationID = muni->applicationID;
1639 pnode->fireMissionIndex = fpdu->fireMissionIndex;
1645 double2float(pnode->munitionStartPoint.c,loc,3);
1655 pnode->firingRange = fpdu->
range;
1658 vector3float2vec3f(delta,&fpdu->
velocity);
1660 vecscale3f(delta,delta,3.0f/1.0f);
1661 vecadd3f(pnode->munitionEndPoint.c,pnode->munitionStartPoint.c,delta);
1663 pnode->_pduchange_fire = TRUE;
1679 if(pnode->collisionType) pnode->isCollided = TRUE;
1680 else pnode->isCollided = FALSE;
1686 pnode->_pduchange_collision = TRUE;
1689 case PDU_DETONATION:
1700 pnode->timestamp = TickTime();
1702 pnode->detonateTime = TickTime();
1717 if(!muni) printf(
"no muni\n");
1719 printf(
"got muni\n");
1720 pnode->munitionEntityID = muni->entityID;
1721 pnode->munitionSiteID = muni->siteID;
1722 pnode->munitionApplicationID = muni->applicationID;
1737 double2float(pnode->detonationLocation.c,loc,3);
1744 vector3float2vec3f(delta,&dpdu->
velocity);
1746 vecscale3f(delta,delta,.5f/1.0f);
1747 veccopy3f(pnode->munitionStartPoint.c,pnode->detonationRelativeLocation.c);
1748 vecadd3f(pnode->munitionEndPoint.c,pnode->munitionStartPoint.c,delta);
1753 if(pnode->articulationParameterArray.n){
1756 int i, np = pnode->articulationParameterArray.n;
1757 ap = dpdu->articulationParameters;
1758 pp = malloc(np *
sizeof(
float));
1763 pp[i] = (float)ap[i].parameterValue;
1767 case 0: pnode->articulationParameterValue0_changed = pp[i];
break;
1768 case 1: pnode->articulationParameterValue1_changed = pp[i];
break;
1769 case 2: pnode->articulationParameterValue2_changed = pp[i];
break;
1770 case 3: pnode->articulationParameterValue3_changed = pp[i];
break;
1771 case 4: pnode->articulationParameterValue4_changed = pp[i];
break;
1772 case 5: pnode->articulationParameterValue5_changed = pp[i];
break;
1773 case 6: pnode->articulationParameterValue6_changed = pp[i];
break;
1774 case 7: pnode->articulationParameterValue7_changed = pp[i];
break;
1779 if(pnode->articulationParameterArray.p) free(pnode->articulationParameterArray.p);
1780 pnode->articulationParameterArray.p = pp;
1783 pnode->_pduchange_detonation = TRUE;
1794int dis_pdus2node_receiver(
struct X3D_Node *node,
struct Vector *pdus){
1800 if(!pdus)
return ihit;
1801 for(i=0;i<pdus->n;i++)
1803 pdu = vector_get(
struct Pdu*,pdus,i);
1804 if(pdu->
padding == TAG_UNCLAIMED)
1811 if(pnode->radioID != rpdu->myRadioCommunicationsFamilyPdu.
radioId)
break;
1815 pnode->timestamp = TickTime();
1823 pnode->_pduchange_receiver = TRUE;
1832int dis_pdus2node_transmitter(
struct X3D_Node *node,
struct Vector *pdus){
1838 if(!pdus)
return ihit;
1839 for(i=0;i<pdus->n;i++)
1841 pdu = vector_get(
struct Pdu*,pdus,i);
1842 if(pdu->
padding == TAG_UNCLAIMED)
1844 case PDU_TRANSMITTER:
1849 if(pnode->radioID != tpdu->myRadioCommunicationsFamilyPdu.
radioId)
break;
1850 if(tpdu->myRadioCommunicationsFamilyPdu.
entityId.
entity != pnode->entityID)
break;
1856 pdu->
padding = TAG_TRANSMITTER;
1858 pnode->timestamp = TickTime();
1862 double2float(pnode->antennaLocation.c,loc,3);
1877 pnode->power = tpdu->
power;
1882 pnode->radioEntityTypeNomenclature = tpdu->
radioEntityType.nomenclature;
1884 pnode->radioID = tpdu->myRadioCommunicationsFamilyPdu.
radioId;
1887 pnode->_pduchange_transmitter = TRUE;
1898int dis_pdus2node_signal(
struct X3D_Node *node,
struct Vector *pdus){
1904 if(!pdus)
return ihit;
1905 for(i=0;i<pdus->n;i++)
1907 pdu = vector_get(
struct Pdu*,pdus,i);
1908 if(pdu->
padding == TAG_UNCLAIMED)
1915 if(pnode->radioID != spdu->myRadioCommunicationsFamilyPdu.
radioId)
break;
1916 if(spdu->myRadioCommunicationsFamilyPdu.
entityId.
entity != pnode->entityID)
break;
1925 pnode->timestamp = TickTime();
1926 if(ONE_INT32_PER_SIGNAL_DATA_BYTE){
1928 unsigned char *cdata = (
unsigned char *)spdu->
data;
1929 pnode->data.p = realloc(pnode->data.p,spdu->
dataLength*
sizeof(
int));
1931 pnode->data.p[k] = (int)(cdata[k]);
1934 memcpy(pnode->data.p,spdu->
data,pnode->dataLength);
1940 pnode->samples = spdu->
samples;
1941 pnode->tdlType = spdu->
tdlType;
1942 pnode->_pduchange_signal = TRUE;
1969struct Vector * dis_node2pdus_sm(
struct X3D_Node *node,
int isHeartbeat){
1973 pdus = newVector(
struct Pdu *, 6);
1977 printf(
"em pduchange create %d remove %d heartbeat %d ticktime %lf\n",pnode->_pduchange_create, pnode->_pduchange_remove, isHeartbeat,TickTime());
1983 struct SimulationManagementPdu *simanpdu;
1986 if(pnode->_pduchange_create){
1994 vector_pushBack(
struct Pdu*,pdus,(
struct Pdu*)crpdu);
1997 if(pnode->_pduchange_remove){
2001 vector_pushBack(
struct Pdu*,pdus,(
struct Pdu*)rmpdu);
2012 if(!pdus)
return ihit;
2013 for(i=0;i<pdus->n;i++)
2015 pdu = vector_get(
struct Pdu*,pdus,i);
2017 case PDU_CREATE_ENTITY:
2022 printf(
"hi from pdu2node create_entity\n");
2023 pdu->
padding = TAG_ENTITY_MANAGER;
2025 pnode->_pduchange_create = TRUE;
2028 case PDU_REMOVE_ENTITY:
2032 printf(
"hi from pdu2node remove_entity\n");
2033 pdu->
padding = TAG_ENTITY_MANAGER;
2035 pnode->_pduchange_remove = TRUE;
2045void dis_set_node_lasttime(
struct X3D_Node *node,
double lasttime){
2047 switch(node->_nodeType){
2048 case NODE_ReceiverPdu:
2049 case NODE_TransmitterPdu:
2050 case NODE_SignalPdu:
2051 case NODE_EspduTransform:
2052 case NODE_DISEntityManager:
2055 pnode->_lasttime = lasttime;
2070 for(i=0;i<pdus->n;i++) {
2071 pdu = vector_get(
struct Pdu*,pdus,i);
2072 if(pdu->
padding == TAG_UNCLAIMED)
2074 case PDU_ENTITY_STATE:
2076 case PDU_TRANSMITTER:
2078 case PDU_ENTITY_STATE_UPDATE:
2080 int j, already_done;
2081 int entityID, siteID, applicationID;
2091 pdu->
padding = TAG_SAME_PROGRAM;
2096 already_done = FALSE;
2099 for(j=0;j<pnode->addEntities.n;j++){
2101 if(candi->_nodeType == NODE_DISEntityTypeMapping){
2104 }
else if(candi->_nodeType == NODE_EspduTransform) {
2111 et->siteID == espdu->
entityID.
site) already_done = TRUE;
2114 if(already_done)
break;
2123 pdu->
padding = TAG_ENTITY_MANAGER;
2129 et = createNewX3DNode0(NODE_EspduTransform);
2132 case PDU_ENTITY_STATE: nodetype = NODE_EspduTransform;
break;
2133 case PDU_RECEIVER: nodetype = NODE_ReceiverPdu;
break;
2134 case PDU_TRANSMITTER: nodetype = NODE_TransmitterPdu;
break;
2135 case PDU_SIGNAL: nodetype = NODE_SignalPdu;
break;
2136 case PDU_ENTITY_STATE_UPDATE: nodetype = NODE_EspduTransform;
break;
2142 et->_nodeType = nodetype;
2146 et->address = newASCIIString(dsock->address);
2147 et->port = dsock->port - fwl_get_testset();
2148 et->multicastRelayHost = newASCIIString(dsock->multicastRelayHost);
2149 et->multicastRelayPort = dsock->multicastRelayPort;
2159 void * pp = pnode->addEntities.p;
2160 pnode->addEntities.p = realloc(pp,
sizeof(
struct X3D_Node*)*upper_power_of_two(pnode->addEntities.n + 1));
2161 pnode->addEntities.p[pnode->addEntities.n] = (
struct X3D_Node*)et;
2162 pnode->addEntities.n++;
2181 if(node->_nodeType == NODE_EspduTransform){
2182 if(pnode && pnode->_nodeType == NODE_DISEntityManager){
2184 static int ADD = 1, REMOVE = 2;
2186 for(i=0;i<pnode->entities.n;i++){
2187 if(pnode->entities.p[i] == node){
2189 AddRemoveChildren(X3D_NODE(pnode), &pnode->entities, (
struct X3D_Node * *)&node, 1, REMOVE,__FILE__,__LINE__);
2190 AddRemoveChildren(X3D_NODE(pnode), &pnode->removedEntities, (
struct X3D_Node * *)&node, 1, ADD,__FILE__,__LINE__);
2200struct Vector * dis_node2pdus(
struct X3D_Node *node,
int isHeartbeat){
2201 struct Vector *pdus = NULL;
2202 switch(node->_nodeType){
2203 case NODE_EspduTransform:
2204 pdus = dis_node2pdus_espdu(node, isHeartbeat);
2206 case NODE_DISEntityManager:
2207 pdus = dis_node2pdus_sm(node,isHeartbeat);
2209 case NODE_ReceiverPdu:
2210 pdus = dis_node2pdus_receiver(node,isHeartbeat);
2212 case NODE_TransmitterPdu:
2213 pdus = dis_node2pdus_transmitter(node,isHeartbeat);
2215 case NODE_SignalPdu:
2216 pdus = dis_node2pdus_signal(node,isHeartbeat);
2222static struct Vector *sockets_send = NULL;
2223static struct Vector *sockets_recv = NULL;
2225struct X3D_Node * dis_find_registered_node_by_entityid(
int entityid,
int sendlist,
int recvlist){
2227 struct X3D_Node *node, *pnode = NULL;
2229 for(i=0;i<sockets_send->n;i++){
2231 if(dsock->registered){
2232 for(j=0;j<dsock->registered->n;j++){
2235 if(node->_nodeType == NODE_EspduTransform){
2237 if(espdu->entityID == entityid){
2246 for(i=0;i<sockets_recv->n;i++){
2248 if(dsock->registered){
2249 for(j=0;j<dsock->registered->n;j++){
2252 if(node->_nodeType == NODE_EspduTransform){
2254 if(espdu->entityID == entityid){
2267struct X3D_Node *dis_find_or_create_espdu_by_category(
int kind,
int domain,
int country,
int category,
int subcategory,
int specific,
int extra){
2268 int i,j,k, ibest, iscore;
2273 for(i=0;i<sockets_recv->n;i++){
2275 if(dsock->registered){
2276 for(j=0;j<dsock->registered->n;j++){
2279 if(node->_nodeType == NODE_DISEntityManager){
2282 for(k=0;k<em->entities.n;k++){
2284 if(bnode->_nodeType == NODE_EspduTransform){
2286 if(domain == bnode->entityDomain) jscore++;
2287 if(category == bnode->entityCategory) jscore++;
2288 if(country == bnode->entityCountry) jscore++;
2289 if(kind == bnode->entityKind) jscore++;
2290 if(extra == bnode->entityExtra) jscore++;
2291 if(subcategory == bnode->entitySubCategory) jscore++;
2292 if(specific == bnode->entitySpecific) jscore++;
2293 if(jscore > iscore){
2310unsigned char buf2[32768];
2312void dis_get_node_lasttime(
struct X3D_Node *node,
double *lasttime,
double *readInterval,
double *writeInterval){
2314 switch(node->_nodeType){
2315 case NODE_ReceiverPdu:
2316 case NODE_TransmitterPdu:
2317 case NODE_SignalPdu:
2318 case NODE_EspduTransform:
2319 case NODE_DISEntityManager:
2322 *lasttime = pnode->_lasttime;
2323 *writeInterval = pnode->writeInterval;
2324 *readInterval = pnode->readInterval;
2331int node_only_transform_changed(
struct X3D_Node *node){
2332 int changed, onlytransform = FALSE;
2334 if( node->_nodeType == NODE_EspduTransform)
2337 changed += pnode->_pduchange_es ? 1 : 0;
2338 changed += pnode->_pduchange_collision ? 2:0;
2339 changed += pnode->_pduchange_fire ? 4:0;
2340 changed += pnode->_pduchange_detonation ? 8:0;
2342 onlytransform = changed == 1;
2343 return onlytransform;
2347int node_pdus_changed_by_scene(
struct X3D_Node *node){
2348 int changed = FALSE;
2349 switch(node->_nodeType){
2350 case NODE_EspduTransform:
2353 changed = pnode->_pduchange_es;
2354 changed |= pnode->_pduchange_collision;
2355 changed |= pnode->_pduchange_fire;
2356 changed |= pnode->_pduchange_detonation;
2359 case NODE_DISEntityManager:
2362 changed = pnode->_pduchange_create;
2363 changed |= pnode->_pduchange_remove;
2366 case NODE_TransmitterPdu:
2369 changed = pnode->_pduchange_transmitter;
2372 case NODE_SignalPdu:
2375 changed = pnode->_pduchange_signal;
2378 case NODE_ReceiverPdu:
2381 changed = pnode->_pduchange_receiver;
2389void reset_node_pduchanged(
struct X3D_Node *node){
2390 switch(node->_nodeType){
2391 case NODE_EspduTransform:
2394 pnode->_pduchange_es = FALSE;
2395 pnode->_pduchange_collision = FALSE;
2396 pnode->_pduchange_fire = FALSE;
2397 pnode->_pduchange_detonation = FALSE;
2400 case NODE_DISEntityManager:
2403 pnode->_pduchange_em_info = FALSE;
2404 pnode->_pduchange_create = FALSE;
2405 pnode->_pduchange_remove = FALSE;
2408 case NODE_TransmitterPdu:
2411 pnode->_pduchange_transmitter = FALSE;
2414 case NODE_SignalPdu:
2417 pnode->_pduchange_signal = FALSE;
2420 case NODE_ReceiverPdu:
2423 pnode->_pduchange_receiver = FALSE;
2432int sockwrite(SOCKET s,
const char *buf,
int len);
2433int sockread(SOCKET s,
const char *buf,
int len);
2434int sockrecvfrom(
struct dis_socket *dsock,
const char *buf,
int len);
2435int socksendto(
struct dis_socket *dsock,
const char *buf,
int len);
2439#define MULTIPLIER 37
2440unsigned int hash37(
const char* str)
2446 for (p = (
unsigned char*)str; *p !=
'\0'; p++)
2447 h = MULTIPLIER * h + *p;
2450const char* getNodeName(
struct X3D_Node* node);
2452void dis_recv_sensor(
int sensorIndex,
int ev,
int butStatus2,
int status,
float* posn3,
float* norm3);
2454 int fromNode, dataNode;
2456 int status, padding;
2457 float posn3[3], norm3[3];
2459static struct Vector* sensor_send_queue = NULL;
2461struct Vector* dis_sensors2pdus() {
2464 struct Vector* pdus = NULL;
2465 if (sensor_send_queue && vectorSize(sensor_send_queue)) {
2466 int nevents = vectorSize(sensor_send_queue);
2467 pdus = newVector(
struct Pdu*, 1);
2469 cpdu = (
struct CommentPdu*)dis_ctor(type_CommentPdu);
2470 vector_pushBack(
struct Pdu*, pdus, (
struct Pdu*)cpdu);
2477 for (
int i = 0; i < nevents; i++) {
2478 struct dis_sensor* ds = vector_get_ptr(
struct dis_sensor, sensor_send_queue, i);
2489void clear_sensor_queue() {
2490 if(sensor_send_queue) sensor_send_queue->n = 0;
2492void dis_send_sensor(
struct X3D_Node* fromNode,
struct X3D_Node* dataNode,
int ev,
int butStatus2,
2493 int status,
float* posn3,
float* norm3) {
2499 if (!sensor_send_queue) sensor_send_queue = newStack(
struct dis_sensor);
2500 struct dis_sensor ds;
2502 ds.butStatus2 = butStatus2;
2504 veccopy3f(ds.posn3, posn3);
2505 veccopy3f(ds.norm3, norm3);
2511 const char* def = getNodeName(fromNode);
2513 printf(
"No DEF name for from nodetype %s", stringNodeType(fromNode->_nodeType));
2516 ds.fromNode = hash37(def == NULL ?
"" : def);
2518 def = getNodeName(dataNode);
2520 printf(
"No DEF name for data nodetype %s", stringNodeType(fromNode->_nodeType));
2523 ds.dataNode = hash37(def);
2526 stack_push(
struct dis_sensor, sensor_send_queue, ds);
2539int getSensorCount();
2540void getSensor(
int k,
struct X3D_Node** fromnode,
struct X3D_Node** datanode);
2542int dis_pdus2sensors(
struct Vector* pdus) {
2552 if (!pdus || !pdus->n)
return ihit;
2553 for (i = 0; i < pdus->n; i++)
2555 pdu = vector_get(
struct Pdu*, pdus, i);
2556 if (pdu->
padding == TAG_UNCLAIMED)
2565 pdu->
padding = TAG_SAME_PROGRAM;
2571 printf(
"%s ",
"COM");
2581 struct dis_sensor* ds = (
struct dis_sensor*)vr[j].variableDatums;
2584 int nsensor = getSensorCount();
2586 if (disverbose()) printf(
"NS%2d ", nsensor);
2587 for (
int k = 0; k < nsensor; k++) {
2588 struct X3D_Node* fromnode, * datanode;
2589 getSensor(k, &fromnode, &datanode);
2591 const char *deff, *defd;
2592 deff = getNodeName(fromnode);
2593 int fromNode, dataNode, OK;
2595 if (!deff) OK = FALSE;
2596 else fromNode = hash37(deff);
2598 defd = getNodeName(datanode);
2599 if (!defd) OK = FALSE;
2600 else dataNode = hash37(defd);
2603 int match = OK && ds->fromNode == fromNode && ds->dataNode == dataNode;
2605 dis_recv_sensor(k, ds->ev, ds->butStatus2, ds->status, ds->posn3, ds->norm3);
2607 if(disverbose()) printf(
"F %s D %s B%d S%d ", deff, defd,ds->butStatus2,ds->status);
2615 if (disverbose()) printf(
"\n");
2624static double last_avatar_position[3] = { 0,0,0 };
2625static double last_avatar_orientation[4] = { 0,0,0,0 };
2626static double avatar_writeInterval = 4.0;
2627struct Vector* dis_avatar2pdus() {
2629 struct Vector* pdus = NULL;
2634 double viewMatrix[16], matinv[16], xyza[4], pointd[3], diff[4];
2635 float xyzaf[4], ypr[3];
2636 viewer_getview(viewMatrix);
2637 vecsetd(pointd, 0.0, 0.0, 0.0);
2638 matinverseAFFINE(matinv, viewMatrix);
2640 transformAFFINEd(pointd, pointd, matinv);
2642 AFFINEmatrix2axisangled(xyza, viewMatrix);
2643 int changed = veclengthd(vecdifd(diff, pointd, last_avatar_position)) > .001 ? 1 : 0;
2644 changed = changed || veclengthd(vecdifd(diff, xyza, last_avatar_orientation)) > .001 ? 1 : 0;
2645 changed = changed || abs(xyza[3] - last_avatar_orientation[3] > .001) ? 1 : 0;
2646 static double last_time = 0.0;
2647 if (last_time == 0.0) last_time = TickTime() - avatar_writeInterval;
2648 double this_time = TickTime();
2649 int heartbeat = FALSE;
2650 if (this_time - last_time > avatar_writeInterval) heartbeat = TRUE;
2651 if (!changed && ! heartbeat) {
2654 last_time = this_time;
2656 veccopyd(last_avatar_position, pointd);
2657 veccopyd(last_avatar_orientation, xyza);
2658 last_avatar_orientation[3] = xyza[3];
2659 pdus = newVector(
struct Pdu*, 6);
2662 static int icount = 0;
2663 if(0) printf(
"Pdu->type = %d i %d\n", ((
struct Pdu*)(espdu))->pduType,icount);
2671 if(0) printf(
"kind %d domain %d country %d category %d sub %d specific %d extra %d\n ",
2679 double2float(xyzaf, xyza, 4);
2680 xyzaf[3] = -xyzaf[3];
2681 axisangle2ypr(xyzaf, ypr);
2686 stack_push(
struct Pdu*, pdus, (
struct Pdu*)espdu);
2691int write_rtp(
unsigned char *buf,
struct X3D_Node *node);
2694 int i,j, nbytes, nb;
2695 if(!sockets_send || sockets_send->n == 0)
return;
2696 thistime = TickTime();
2697 for(i=0;i<sockets_send->n;i++){
2702 if(dsock->registered){
2703 for(j=0;j<dsock->registered->n;j++){
2708 double lasttime, dtime, readInterval, writeInterval, isHeartbeat;
2711 dis_get_node_lasttime(node,&lasttime,&readInterval,&writeInterval);
2712 if(writeInterval == 0.0)
continue;
2715 dtime = thistime - lasttime;
2716 isHeartbeat = dtime > writeInterval ? TRUE: FALSE;
2717 if(!isHeartbeat && !node_pdus_changed_by_scene(node))
continue;
2723 lasttime = thistime;
2724 dsock->lasttime = thistime;
2726 nb = write_rtp(&buf2[nbytes],node);
2731 pdus = dis_node2pdus(node,isHeartbeat);
2733 dis_set_node_lasttime(node,lasttime);
2735 if(pdus && pdus->n) {
2736 struct Pdu* pdu = vector_get(
struct Pdu*,pdus,0);
2739 nb = dis_write_stream(&buf2[nbytes],pdus);
2742 printf(
"sendloop >>>>\n");
2743 print_stream(&buf2[nbytes], nb);
2744 printf(
"<<<< sendloop\n");
2747 reset_node_pduchanged(node);
2750 pdus = dis_sensors2pdus();
2751 nb = dis_write_stream(&buf2[nbytes], pdus);
2752 clear_sensor_queue();
2754 pdus = dis_avatar2pdus();
2755 nb = dis_write_stream(&buf2[nbytes], pdus);
2757 if (nbytes) socksendto(dsock, (
const char *)buf2, nbytes);
2776 unsigned char toprow[2];
2777 unsigned short sequence;
2778 unsigned int timestamp;
2781unsigned char * rtp_strip_header(
unsigned char *buf,
int *heard){
2782 unsigned char *carat = buf;
2786 if(carat[0] > 127) {
2788 struct rtp_header rtph;
2791 memcpy(&rtph.toprow,carat,2);
2793 memcpy(&rtph.sequence,carat,2);
2795 memcpy(&rtph.timestamp,carat,4);
2797 memcpy(&rtph.ssrc,carat,4);
2799 cc = rtph.toprow[0] << 4 >> 4;
2800 x = rtph.toprow[0] & 1 << 4;
2812unsigned char * rtp_add_header(
unsigned char *buf,
unsigned int timestamp){
2813 struct rtp_header rtph;
2814 unsigned char PayloadType;
2815 unsigned char *carat = buf;
2817 memset(&rtph,0,
sizeof(
struct rtp_header));
2818 rtph.toprow[0] = 2 << 7 | 0 << 6 | 0 << 5 | 0;
2819 rtph.toprow[1] = 0 << 7 | PayloadType;
2820 rtph.sequence = htons(0);
2821 rtph.timestamp = htonl(timestamp);
2823 memcpy(carat,&rtph.toprow,2);
2825 memcpy(carat,&rtph.sequence,2);
2827 memcpy(carat,&rtph.timestamp,4);
2829 memcpy(carat,&rtph.ssrc,4);
2833int write_rtp(
unsigned char *buf,
struct X3D_Node *node){
2836 switch(node->_nodeType){
2837 case NODE_EspduTransform:
2840 case NODE_DISEntityManager:
2843 case NODE_ReceiverPdu:
2846 case NODE_TransmitterPdu:
2849 case NODE_SignalPdu:
2856 unsigned char *carat;
2857 unsigned int hours, timestamp;
2858 TickTime2DISTime(TickTime(),1,&hours,×tamp);
2859 carat = rtp_add_header(buf,timestamp);
2864void set_rtp_heard(
struct X3D_Node *node){
2868 switch(node->_nodeType){
2869 case NODE_EspduTransform:
2872 case NODE_DISEntityManager:
2875 case NODE_ReceiverPdu:
2878 case NODE_TransmitterPdu:
2881 case NODE_SignalPdu:
2888void dis_set_isActive(
struct X3D_Node*node,
int ival){
2889 switch(node->_nodeType){
2890 case NODE_EspduTransform:
2893 if(pnode->isActive != ival){
2894 pnode->isActive = ival;
2899 case NODE_DISEntityManager:
2902 if(pnode->isActive != ival){
2903 pnode->isActive = ival;
2908 case NODE_ReceiverPdu:
2911 if(pnode->isActive != ival){
2912 pnode->isActive = ival;
2917 case NODE_TransmitterPdu:
2920 if(pnode->isActive != ival){
2921 pnode->isActive = ival;
2926 case NODE_SignalPdu:
2929 if(pnode->isActive != ival){
2930 pnode->isActive = ival;
2940void dis_set_isNetworkMode(
struct X3D_Node*node,
int networkMode){
2941 int isStandAlone, isNetworkReader, isNetworkWriter;
2942 isStandAlone = isNetworkReader = isNetworkWriter = 0;
2943 switch(networkMode){
2944 case 0: isStandAlone = TRUE;
break;
2945 case 1: isNetworkReader = TRUE;
break;
2946 case 2: isNetworkWriter = TRUE;
break;
2949 switch(node->_nodeType){
2950 case NODE_EspduTransform:
2953 if(pnode->isStandAlone != isStandAlone){
2954 pnode->isStandAlone = isStandAlone;
2957 if(pnode->isNetworkReader != isNetworkReader){
2958 pnode->isNetworkReader = isNetworkReader;
2961 if(pnode->isNetworkWriter != isNetworkWriter){
2962 pnode->isNetworkWriter = isNetworkWriter;
2967 case NODE_DISEntityManager:
2970 if(pnode->isStandAlone != isStandAlone){
2971 pnode->isStandAlone = isStandAlone;
2974 if(pnode->isNetworkReader != isNetworkReader){
2975 pnode->isNetworkReader = isNetworkReader;
2978 if(pnode->isNetworkWriter != isNetworkWriter){
2979 pnode->isNetworkWriter = isNetworkWriter;
2984 case NODE_ReceiverPdu:
2987 if(pnode->isStandAlone != isStandAlone){
2988 pnode->isStandAlone = isStandAlone;
2991 if(pnode->isNetworkReader != isNetworkReader){
2992 pnode->isNetworkReader = isNetworkReader;
2995 if(pnode->isNetworkWriter != isNetworkWriter){
2996 pnode->isNetworkWriter = isNetworkWriter;
3001 case NODE_TransmitterPdu:
3004 if(pnode->isStandAlone != isStandAlone){
3005 pnode->isStandAlone = isStandAlone;
3008 if(pnode->isNetworkReader != isNetworkReader){
3009 pnode->isNetworkReader = isNetworkReader;
3012 if(pnode->isNetworkWriter != isNetworkWriter){
3013 pnode->isNetworkWriter = isNetworkWriter;
3018 case NODE_SignalPdu:
3021 if(pnode->isStandAlone != isStandAlone){
3022 pnode->isStandAlone = isStandAlone;
3025 if(pnode->isNetworkReader != isNetworkReader){
3026 pnode->isNetworkReader = isNetworkReader;
3029 if(pnode->isNetworkWriter != isNetworkWriter){
3030 pnode->isNetworkWriter = isNetworkWriter;
3041int dis_read_stream(
unsigned char * datastream,
int streamsize,
struct Vector *pdus,
int *heard)
3043 int pdutype, bytesread;
3044 unsigned char *carat, *carat2;
3045 static char pdubuffer[10000];
3046 unsigned char *pdubuf;
3049 carat = &datastream[0];
3050 carat = rtp_strip_header(carat,heard);
3051 while(bytesread < streamsize){
3052 int i, distype, nbytes;
3053 if(0)
for(i=0;i<210;i+=10){
3057 printf(
"%5d",(
int)carat[i+j]);
3061 pdutype = (int)(carat[2]);
3062 distype = pduToDis(pdutype);
3065 pdubuf = dis_ctor(distype);
3066 carat2 = dis_unmarshal(carat,pdubuf,distype);
3067 nbytes = (carat2 - carat);
3068 pdu = (
struct Pdu*)pdubuf;
3071 vector_pushBack(
struct Pdu*,pdus,pdu);
3076 unsigned char buf3[32000];
3077 unsigned char *carat3;
3079 carat3 = dis_marshal(buf3,pdubuf,distype);
3080 b3size = carat3 - buf3;
3081 printf(
"marshed bits %d bytes %d\n",b3size*8,b3size);
3083 if(memcmp(carat,buf3,b3size) == 0)
3087 for(i=0;i<210;i+=10){
3091 printf(
"%5d",(
int)buf3[i+j]);
3100 if(0)
if(pdutype == 1){
3103 printf(
"loc %lf %lf %lf rot %f %f %f\n",
3107 n = p->numberOfVariableParameters;
3124 struct ArticulatedParts *v;
3125 v = (
struct ArticulatedParts*)p->variableParameters;
3126 printf(
"v address = %p\n",v);
3129 printf(
"%d %d %d %d %d %lf\n",
3131 (
int)v[i].recordType,
3132 (
int)v[i].changeIndicator,
3133 (
int)v[i].partAttachedTo,
3134 (
int)v[i].parameterType,
3141 printf(
"v address = %p\n",v);
3144 printf(
"%d %d %d %d %d %lf\n",
3146 (
int)v[i].parameterTypeDesignator,
3147 (
int)v[i].changeIndicator,
3148 (
int)v[i].partAttachedTo,
3149 (
int)v[i].parameterType,
3157 bytesread += nbytes;
3173int dis_write_stream(
unsigned char * datastream,
struct Vector *pdus)
3177 unsigned char *carat;
3180 if(pdus && pdus->n){
3181 for(i=0;i<pdus->n;i++){
3182 struct Pdu *pdu = vector_get(
struct Pdu*,pdus,i);
3184 int distype = pduToDis(pdu->
pduType);
3185 carat = dis_marshal(carat,(
unsigned char*)pdu,distype);
3189 nbytes = (int)(carat - datastream);
3194struct X3D_Node* findNodeByName(
char* defname) {
3202 context = X3D_PROTO(root);
3204 if (context->__DEFnames) {
3205 int ndefs = vectorSize(context->__DEFnames);
3206 for (
int i = 0; i < ndefs; i++) {
3207 def = vector_get(
struct brotoDefpair, context->__DEFnames, i);
3209 if (!strcmp(def.name,defname)) {
3458static double lasttime;
3459static char buf[32768];
3460static struct Vector *pdus = NULL;
3469 int i,j,nbytes, more, heard;
3470 static int count = 0;
3471 double thistime, dtime;
3472 if(!sockets_recv || sockets_recv->n == 0)
return;
3473 thistime = TickTime();
3474 dtime = thistime - lasttime;
3475 if(!pdus) pdus = newVector(
struct Pdu*,20);
3479 for(i=0;i<sockets_recv->n;i++){
3486 nbytes = sockrecvfrom(dsock,buf,32000);
3490 dsock->lasttime = thistime;
3493 for(j=0;j<pdus->n;j++){
3494 struct Pdu* pdu = vector_get(
struct Pdu*,pdus,j);
3495 dis_dtor((
unsigned char *)pdu,pduToDis(pdu->pduType));
3498 dis_read_stream((
unsigned char *)buf,nbytes,pdus,&heard);
3499 for (
int j = 0; j < pdus->n; j++) {
3500 struct Pdu* pdu = vector_get(
struct Pdu*, pdus, j);
3509 if(dsock->registered){
3511 for(j=0;j<dsock->registered->n;j++){
3516 switch(node->_nodeType){
3517 case NODE_EspduTransform:
3518 ihit = dis_pdus2node_espdu(node, pdus);
3520 case NODE_DISEntityManager:
3522 ihit = dis_pdus2node_sm(node, pdus);
3524 case NODE_ReceiverPdu:
3525 ihit = dis_pdus2node_receiver(node, pdus);
3527 case NODE_TransmitterPdu:
3528 ihit = dis_pdus2node_transmitter(node, pdus);
3530 case NODE_SignalPdu:
3531 ihit = dis_pdus2node_signal(node, pdus);
3537 if(heard) set_rtp_heard(node);
3538 dis_set_isActive(node,TRUE);
3539 dis_set_node_lasttime(node,thistime);
3545 ihit = dis_pdus2sensors(pdus);
3548 int counts[9] = { 0,0,0,0,0,0,0,0,0 };
3549 for (
int j = 0; j < pdus->n; j++) {
3550 struct Pdu* pdu = vector_get(
struct Pdu*, pdus, j);
3554 if(0) printf(
"counts U%d S%d E%d M%d r%d t%d s%d A%d S%d\n",
3555 counts[0], counts[1], counts[2], counts[3], counts[4], counts[5], counts[6], counts[7], counts[8]);
3558 if(0) printf(
"leftovers %d avatarhits %d pdus %d",pdus->n - nhit, ihit, pdus->n);
3559 nhit = dis_pdus2newnode(dsock,sockem, pdus);
3560 if(0) printf(
" %d used\n",nhit);
3564 if(dsock->registered){
3565#define RETIRE_TIME 300.0
3569 for(j=0;j<dsock->registered->n;j++){
3571 if(node->_nodeType == NODE_DISEntityManager){
3576 for (j = 0; j < dsock->registered->n; j++) {
3578 struct X3D_Node* node = vector_get(
struct X3D_Node*, dsock->registered, j);
3579 if (node->_nodeType != NODE_DISEntityManager) {
3580 double readinterval, writeinterval, lasttime;
3581 dis_get_node_lasttime(node, &lasttime, &readinterval, &writeinterval);
3583 double timeout = RETIRE_TIME;
3584 if (node->_nodeType == NODE_EspduTransform) {
3586 if (!espdu->isNetworkWriter) {
3590 if (thistime - lasttime > timeout) {
3592 dis_set_isActive(node, FALSE);
3596 if (thistime - lasttime > (timeout * 3)) {
3599 if (sockem && node->_nodeType == NODE_EspduTransform) {
3600 ihit = dis_entity_retire(sockem, node);
3603 printf(
" retired one\n");
3610 if(sockem && sockem->removedEntities.n) {
3620void dis_open_socket(
struct dis_socket* dsock){
3621 if(dsock->multicastRelayHost && strlen(dsock->multicastRelayHost)){
3622 printf(
"[%s]\n",dsock->multicastRelayHost);
3630void * dis_register(
struct X3D_Node* node,
char *address,
int applicationID,
int entityID,
char *multicastRelayHost,
3631 int multicastRelayPort,
3632 char *networkMode,
int port,
double readInterval,
int rtpHeaderExpected,
int siteID,
double writeInterval){
3634 int inetworkmode = 0;
3635 int sport = port + fwl_get_testset();
3636 if(!strcmp(networkMode,
"standAlone")) inetworkmode = 0;
3637 else if(!strcmp(networkMode,
"networkReader")) inetworkmode = 1;
3638 else if(!strcmp(networkMode,
"networkWriter")) inetworkmode = 2;
3640 if(inetworkmode == 0) preg = NULL;
3641 if(inetworkmode == 1){
3644 if(!sockets_recv) sockets_recv = newVector(
struct dis_socket,10);
3648 for(i=0;i<sockets_recv->n;i++){
3649 dsock = vector_get_ptr(
struct dis_socket,sockets_recv,i);
3650 if(!strcmp(dsock->address,address) && dsock->port == sport){
3664 vector_pushBack(
struct dis_socket,sockets_recv,asock);
3665 dsock = vector_get_ptr(
struct dis_socket,sockets_recv,sockets_recv->n-1);
3666 dsock->address = address;
3667 dsock->port = sport;
3668 dsock->multicastRelayHost = multicastRelayHost;
3669 dsock->multicastRelayPort = multicastRelayPort;
3670 dsock->idir = inetworkmode;
3672 dis_open_socket(dsock);
3675 if(!dsock->registered) dsock->registered = newVector(
struct X3D_Node*,20);
3676 vector_pushBack(
struct X3D_Node*,dsock->registered,node);
3677 preg = (
void*)dsock;
3679 if(inetworkmode==2){
3682 if(!sockets_send) sockets_send = newVector(
struct dis_socket,10);
3686 for(i=0;i<sockets_send->n;i++){
3687 dsock = vector_get_ptr(
struct dis_socket,sockets_send,i);
3688 if(!strcmp(dsock->address,address) && dsock->port == sport){
3702 vector_pushBack(
struct dis_socket,sockets_send,asock);
3703 dsock = vector_get_ptr(
struct dis_socket,sockets_send,sockets_send->n-1);
3704 dsock->address = address;
3705 dsock->port = sport;
3706 dsock->multicastRelayHost = multicastRelayHost;
3707 dsock->multicastRelayPort = multicastRelayPort;
3708 dsock->idir = inetworkmode;
3710 dis_open_socket(dsock);
3713 if(!dsock->registered) dsock->registered = newVector(
struct X3D_Node*,20);
3714 vector_pushBack(
struct X3D_Node*,dsock->registered,node);
3715 preg = (
void*)dsock;
3717 dis_set_isNetworkMode(node, inetworkmode);
3723 if(dsock->registered){
3724 for(j=0;j<dsock->registered->n;j++){
3725 if(vector_get(
struct X3D_Node*,dsock->registered,j)==node){
3726 vector_remove_elem(
struct X3D_Node*,dsock->registered,j);
3732int dis_check_socket_change(
struct dis_socket* dsock,
char *address,
int port,
3733 char *multicastRelayHost,
int multicastRelayPort,
char *networkMode){
3742 int inetworkmode = 0;
3743 int sport = port + fwl_get_testset();
3744 if(!strcmp(networkMode,
"standAlone")) inetworkmode = 0;
3745 else if(!strcmp(networkMode,
"networkReader")) inetworkmode = 1;
3746 else if(!strcmp(networkMode,
"networkWriter")) inetworkmode = 2;
3747 if(inetworkmode == 0 && dsock == NULL)
return FALSE;
3748 if(dsock == NULL)
return TRUE;
3749 if(inetworkmode != dsock->idir)
return TRUE;
3750 if(strcmp(address,dsock->address))
return TRUE;
3751 if(sport != dsock->port)
return TRUE;
3752 if(strcmp(multicastRelayHost,dsock->multicastRelayHost))
return TRUE;
3753 if(multicastRelayPort != dsock->multicastRelayPort)
return TRUE;
3788 unsigned char *src, *dest;
3789 src = (
unsigned char *)original;
3790 dest = (
unsigned char *)copy;
3792 offset = NODE_OFFSETS[original->_nodeType];
3793 while(offset[0] > -1){
3797 union anyVrml *anysrc, *anydest;
3798 anysrc = (
union anyVrml*)(src + offset[1]);
3799 anydest = (
union anyVrml*)(dest + offset[1]);
3800 shallow_copy_field(offset[2],anysrc,anydest);
3805int shallow_compare_field(
int typeIndex,
union anyVrml* source,
union anyVrml* dest)
3807 int i, isize, has_changed;
3810 has_changed = FALSE;
3812 isMF = typeIndex % 2;
3813 sftype = typeIndex - isMF;
3816 isize = sizeofSForMF(sftype);
3824 if(mfs->n != mfd->n){
3827 ps = (
char *)mfs->p;
3828 pd = (
char *)mfd->p;
3829 for(i=0;i<mfs->n;i++)
3831 has_changed = shallow_compare_field(sftype,(
union anyVrml*)ps,(
union anyVrml*)pd);
3840 case FIELDTYPE_SFString:
3850 has_changed = memcmp(*sd,*ss,
sizeof(
struct Uni_String)) ? TRUE : FALSE;
3857 has_changed = memcmp(dest,source,isize) ? TRUE : FALSE;
3864int shallow_compare_node_fields(
struct X3D_Node *node,
struct X3D_Node *old,
const int *PFIELDS){
3865 const int *fname, *offset;
3866 unsigned char *src, *dest;
3869 src = (
unsigned char *)old;
3870 dest = (
unsigned char *)node;
3874 while(fname[k] > -1){
3875 offset = NODE_OFFSETS[node->_nodeType];
3876 while(offset[0] > -1){
3877 if(offset[0] == fname[k]){
3878 union anyVrml *anysrc, *anydest;
3879 anysrc = (
union anyVrml*)(src + offset[1]);
3880 anydest = (
union anyVrml*)(dest + offset[1]);
3881 has_changed += shallow_compare_field(offset[2],anysrc,anydest);
3888 return has_changed ? TRUE : FALSE;
3891int mark_changed_node_fields(
struct X3D_Node *node,
struct X3D_Node *old,
const int *PFIELDS){
3892 const int *fname, *offset;
3893 unsigned char *src, *dest;
3896 src = (
unsigned char *)old;
3897 dest = (
unsigned char *)node;
3901 while(fname[k] > -1){
3902 offset = NODE_OFFSETS[node->_nodeType];
3903 while(offset[0] > -1){
3904 if(offset[0] == fname[k]){
3905 union anyVrml *anysrc, *anydest;
3906 anysrc = (
union anyVrml*)(src + offset[1]);
3907 anydest = (
union anyVrml*)(dest + offset[1]);
3908 if(shallow_compare_field(offset[2],anysrc,anydest)){
3909 MARK_EVENT(node,offset[1]);
3923const int FIELDS_networksensor [] = {
3925 FIELDNAMES_isActive,
3926 FIELDNAMES_timestamp,
3929 FIELDNAMES_multicastRelayHost,
3930 FIELDNAMES_multicastRelayPort,
3931 FIELDNAMES_networkMode,
3932 FIELDNAMES_isNetworkReader,
3933 FIELDNAMES_isNetworkWriter,
3934 FIELDNAMES_isStandAlone,
3935 FIELDNAMES_readInterval,
3936 FIELDNAMES_writeInterval,
3937 FIELDNAMES_rtpHeaderExpected,
3938 FIELDNAMES_isRtpHeaderHeard,
3945const int FIELDS_entity [] = {
3946 FIELDNAMES_entityID,
3947 FIELDNAMES_applicationID,
3952const int FIELDS_geo [] = {
3953 FIELDNAMES_geoSystem,
3954 FIELDNAMES_geoCoords,
3957const int FIELDS_geosys [] = {
3958 FIELDNAMES_geoSystem,
3961const int FIELDS_geocoord [] = {
3962 FIELDNAMES_geoCoords,
3967const int FIELDS_em_info [] = {
3968 FIELDNAMES_entityCategory,
3969 FIELDNAMES_entityCountry,
3970 FIELDNAMES_entityDomain,
3971 FIELDNAMES_entityExtra,
3972 FIELDNAMES_entityKind,
3973 FIELDNAMES_entitySpecific,
3974 FIELDNAMES_entitySubCategory,
3977const int FIELDS_create [] = {
3978 FIELDNAMES_addedEntities,
3981const int FIELDS_remove [] = {
3982 FIELDNAMES_removedEntities,
3986const int FIELDS_es_force [] = {
3992const int FIELDS_es_transform [] = {
3994 FIELDNAMES_children,
3995 FIELDNAMES_rotation,
3997 FIELDNAMES_scaleOrientation,
3998 FIELDNAMES_translation,
4005const int FIELDS_es_deadreckoning [] = {
4006 FIELDNAMES_deadReckoning,
4007 FIELDNAMES_linearVelocity,
4008 FIELDNAMES_linearAcceleration,
4012const int FIELDS_es_articulation [] = {
4013 FIELDNAMES_set_articulationParameterValue0,
4014 FIELDNAMES_set_articulationParameterValue1,
4015 FIELDNAMES_set_articulationParameterValue2,
4016 FIELDNAMES_set_articulationParameterValue3,
4017 FIELDNAMES_set_articulationParameterValue4,
4018 FIELDNAMES_set_articulationParameterValue5,
4019 FIELDNAMES_set_articulationParameterValue6,
4020 FIELDNAMES_set_articulationParameterValue7,
4021 FIELDNAMES_articulationParameterCount,
4022 FIELDNAMES_articulationParameterDesignatorArray,
4023 FIELDNAMES_articulationParameterChangeIndicatorArr,
4024 FIELDNAMES_articulationParameterIdPartAttachedToAr,
4025 FIELDNAMES_articulationParameterTypeArray,
4026 FIELDNAMES_articulationParameterArray,
4027 FIELDNAMES_articulationParameterValue0_changed,
4028 FIELDNAMES_articulationParameterValue1_changed,
4029 FIELDNAMES_articulationParameterValue2_changed,
4030 FIELDNAMES_articulationParameterValue3_changed,
4031 FIELDNAMES_articulationParameterValue4_changed,
4032 FIELDNAMES_articulationParameterValue5_changed,
4033 FIELDNAMES_articulationParameterValue6_changed,
4034 FIELDNAMES_articulationParameterValue7_changed,
4038const int FIELDS_collision [] = {
4039 FIELDNAMES_collisionType,
4040 FIELDNAMES_collideTime,
4041 FIELDNAMES_isCollided,
4045const int FIELDS_events [] = {
4046 FIELDNAMES_eventEntityID,
4047 FIELDNAMES_eventApplicationID,
4048 FIELDNAMES_eventSiteID,
4049 FIELDNAMES_eventNumber,
4053const int FIELDS_fire [] = {
4056 FIELDNAMES_fireMissionIndex,
4057 FIELDNAMES_firingRange,
4058 FIELDNAMES_firedTime,
4062const int FIELDS_detonation [] = {
4063 FIELDNAMES_detonationLocation,
4064 FIELDNAMES_detonationRelativeLocation,
4065 FIELDNAMES_detonationResult,
4066 FIELDNAMES_detonateTime,
4067 FIELDNAMES_isDetonated,
4071const int FIELDS_munition [] = {
4072 FIELDNAMES_munitionEntityID,
4073 FIELDNAMES_munitionApplicationID,
4074 FIELDNAMES_munitionSiteID,
4075 FIELDNAMES_munitionStartPoint,
4076 FIELDNAMES_munitionEndPoint,
4077 FIELDNAMES_munitionQuantity,
4080const int FIELDS_rate [] = {
4081 FIELDNAMES_firingRate,
4087const int FIELDS_receiver [] = {
4089 FIELDNAMES_whichGeometry,
4090 FIELDNAMES_receiverState,
4091 FIELDNAMES_receivedPower,
4092 FIELDNAMES_transmitterEntityID,
4093 FIELDNAMES_transmitterApplicationID,
4094 FIELDNAMES_transmitterSiteID,
4095 FIELDNAMES_transmitterRadioID,
4099const int FIELDS_signal [] = {
4101 FIELDNAMES_whichGeometry,
4103 FIELDNAMES_dataLength,
4104 FIELDNAMES_encodingScheme,
4105 FIELDNAMES_sampleRate,
4111const int FIELDS_transmitter [] = {
4113 FIELDNAMES_whichGeometry,
4114 FIELDNAMES_radioEntityTypeCategory,
4115 FIELDNAMES_radioEntityTypeCountry,
4116 FIELDNAMES_radioEntityTypeDomain,
4117 FIELDNAMES_radioEntityTypeKind,
4118 FIELDNAMES_radioEntityTypeNomenclature,
4119 FIELDNAMES_radioEntityTypeNomenclatureVersion,
4120 FIELDNAMES_antennaLocation,
4121 FIELDNAMES_antennaPatternLength,
4122 FIELDNAMES_antennaPatternType,
4123 FIELDNAMES_relativeAntennaLocation,
4124 FIELDNAMES_inputSource,
4125 FIELDNAMES_transmitState,
4127 FIELDNAMES_frequency,
4128 FIELDNAMES_transmitFrequencyBandwidth,
4129 FIELDNAMES_lengthOfModulationParameters,
4130 FIELDNAMES_modulationTypeDetail,
4131 FIELDNAMES_modulationTypeMajor,
4132 FIELDNAMES_modulationTypeMajor,
4133 FIELDNAMES_modulationTypeSpreadSpectrum,
4134 FIELDNAMES_modulationTypeSystem,
4135 FIELDNAMES_cryptoSystem,
4136 FIELDNAMES_cryptoKeyID,
4141 if(node->_oldState == NULL){
4145 old = createNewX3DNode0(node->_nodeType);
4147 node->_oldState = old;
4149 if(!node->_registered){
4151 node->address->strptr = strdup( fwl_get_DISaddress() ? fwl_get_DISaddress() : node->address->strptr );
4157 node->multicastRelayHost->strptr = strdup(
"");
4158 node->port = fwl_get_DISport()? fwl_get_DISport():node->port;
4159 node->siteID = fwl_get_DISsite()? fwl_get_DISsite():node->siteID ;
4160 node->applicationID = fwl_get_DISapplication() ? fwl_get_DISapplication() : node->applicationID;
4162 psock = dis_register(X3D_NODE(node),
4163 node->address->strptr,
4164 node->applicationID,
4165 node->entityID, node->multicastRelayHost->strptr, node->multicastRelayPort,
4166 node->networkMode->strptr, node->port,
4167 node->readInterval, node->rtpHeaderExpected,
4169 node->writeInterval);
4170 node->_registered = TRUE;
4171 node->_dsock = psock;
4173 if(node->_registered){
4176 if(shallow_compare_node_fields(X3D_NODE(node),node->_oldState,FIELDS_networksensor)){
4178 changed = dis_check_socket_change((
struct dis_socket*)node->_dsock,node->address->strptr, node->port,
4179 node->multicastRelayHost->strptr,node->multicastRelayPort, node->networkMode->strptr);
4181 dis_unregister((
struct dis_socket*)node->_dsock,X3D_NODE(node));
4182 node->_registered = FALSE;
4183 node->_dsock = NULL;
4197 if(!node->__geoSystem || shallow_compare_node_fields(X3D_NODE(node),node->_oldState,FIELDS_geosys)){
4198 compile_geoSystem(X3D_NODE(node),node->_nodeType,&node->geoSystem,&node->__geoSystem);
4199 update_origin(GEOSYS(node->__geoSystem), X3D_NODE(node), &node->geoCoords, NULL);
4205 compile_DIS_network(node);
4206 compile_DIS_geo(node);
4209 if(node->_oldState == NULL){
4213 old = createNewX3DNode0(node->_nodeType);
4215 node->_oldState = old;
4217 if(!node->_registered){
4219 psock = dis_register(X3D_NODE(node),node->address->strptr,node->applicationID,node->entityID,node->multicastRelayHost->strptr,
4220 node->multicastRelayPort,
4221 node->networkMode->strptr, node->port,node->readInterval,node->rtpHeaderExpected,node->siteID,node->writeInterval);
4222 node->_registered = TRUE;
4223 node->_dsock = psock;
4225 if(node->_registered){
4228 if(shallow_compare_node_fields(X3D_NODE(node),node->_oldState,FIELDS_networksensor)){
4230 changed = dis_check_socket_change((
struct dis_socket*)node->_dsock,node->address->strptr, node->port,
4231 node->multicastRelayHost->strptr,node->multicastRelayPort, node->networkMode->strptr);
4233 dis_unregister((
struct dis_socket*)node->_dsock,X3D_NODE(node));
4234 node->_registered = FALSE;
4235 node->_dsock = NULL;
4247 if(!node->__geoSystem || shallow_compare_node_fields(X3D_NODE(node),node->_oldState,FIELDS_geosys)){
4248 compile_geoSystem(X3D_NODE(node),node->_nodeType,&node->geoSystem,&node->__geoSystem);
4249 update_origin(GEOSYS(node->__geoSystem), X3D_NODE(node), &node->geoCoords, NULL);
4255 if(node->isNetworkReader){
4256 mark_changed_node_fields(X3D_NODE(node), node->_oldState, FIELDS_transmitter);
4257 }
else if(node->isNetworkWriter){
4258 if(shallow_compare_node_fields(X3D_NODE(node),node->_oldState,FIELDS_transmitter)){
4259 node->_pduchange_transmitter = TRUE;
4262 freeMallocedNodeFields(node->_oldState);
4263 shallow_copy_node(node->_oldState,X3D_NODE(node));
4266 if(node->isNetworkReader){
4267 mark_changed_node_fields(X3D_NODE(node), node->_oldState, FIELDS_signal);
4268 }
else if(node->isNetworkWriter){
4269 if(shallow_compare_node_fields(X3D_NODE(node),node->_oldState,FIELDS_signal)){
4270 node->_pduchange_signal = TRUE;
4273 freeMallocedNodeFields(node->_oldState);
4274 shallow_copy_node(node->_oldState,X3D_NODE(node));
4277 if(node->isNetworkReader){
4278 mark_changed_node_fields(X3D_NODE(node), node->_oldState, FIELDS_receiver);
4279 }
else if(node->isNetworkWriter){
4280 if(shallow_compare_node_fields(X3D_NODE(node),node->_oldState,FIELDS_receiver)){
4281 node->_pduchange_receiver = TRUE;
4284 freeMallocedNodeFields(node->_oldState);
4285 shallow_copy_node(node->_oldState,X3D_NODE(node));
4292 if(node->isNetworkReader){
4293 if(node->_pduchange_es){
4294 mark_changed_node_fields(X3D_NODE(node), node->_oldState, FIELDS_em_info);
4295 mark_changed_node_fields(X3D_NODE(node), node->_oldState, FIELDS_es_force);
4296 mark_changed_node_fields(X3D_NODE(node), node->_oldState, FIELDS_es_deadreckoning);
4297 mark_changed_node_fields(X3D_NODE(node), node->_oldState, FIELDS_es_articulation);
4300 if(node->_pduchange_collision){
4301 mark_changed_node_fields(X3D_NODE(node), node->_oldState, FIELDS_collision);
4303 if(node->_pduchange_fire){
4304 mark_changed_node_fields(X3D_NODE(node), node->_oldState, FIELDS_fire);
4306 if(node->_pduchange_fire || node->_pduchange_collision){
4309 if(node->_pduchange_detonation){
4310 mark_changed_node_fields(X3D_NODE(node), node->_oldState, FIELDS_detonation);
4312 if(node->_pduchange_fire || node->_pduchange_detonation){
4313 mark_changed_node_fields(X3D_NODE(node), node->_oldState, FIELDS_munition);
4314 mark_changed_node_fields(X3D_NODE(node), node->_oldState, FIELDS_rate);
4317 reset_node_pduchanged(X3D_NODE(node));
4319 }
else if(node->isNetworkWriter){
4320 int es_info, es_force, es_deadreckoning, es_articulation;
4321 es_info = es_force = es_deadreckoning = es_articulation = FALSE;
4322 if(shallow_compare_node_fields(X3D_NODE(node),node->_oldState,FIELDS_em_info)){
4325 if(shallow_compare_node_fields(X3D_NODE(node),node->_oldState,FIELDS_es_force)){
4328 if(shallow_compare_node_fields(X3D_NODE(node),node->_oldState,FIELDS_es_deadreckoning)){
4329 es_deadreckoning = FALSE;
4331 if(shallow_compare_node_fields(X3D_NODE(node),node->_oldState,FIELDS_es_articulation)){
4334 es_articulation = TRUE;
4335 node->articulationParameterArray.p = realloc(node->articulationParameterArray.p,16*
sizeof(
float));
4336 n = node->articulationParameterArray.n;
4340 if(node->set_articulationParameterValue0 != old->set_articulationParameterValue0)
4341 node->articulationParameterArray.p[i] = node->set_articulationParameterValue0;
4345 if(node->set_articulationParameterValue0 != old->set_articulationParameterValue1)
4346 node->articulationParameterArray.p[i] = node->set_articulationParameterValue1;
4350 if(node->set_articulationParameterValue0 != old->set_articulationParameterValue2)
4351 node->articulationParameterArray.p[i] = node->set_articulationParameterValue2;
4355 if(node->set_articulationParameterValue0 != old->set_articulationParameterValue3)
4356 node->articulationParameterArray.p[i] = node->set_articulationParameterValue3;
4360 if(node->set_articulationParameterValue0 != old->set_articulationParameterValue4)
4361 node->articulationParameterArray.p[i] = node->set_articulationParameterValue4;
4365 if(node->set_articulationParameterValue0 != old->set_articulationParameterValue5)
4366 node->articulationParameterArray.p[i] = node->set_articulationParameterValue5;
4370 if(node->set_articulationParameterValue0 != old->set_articulationParameterValue6)
4371 node->articulationParameterArray.p[i] = node->set_articulationParameterValue6;
4375 if(node->set_articulationParameterValue0 != old->set_articulationParameterValue7)
4376 node->articulationParameterArray.p[i] = node->set_articulationParameterValue7;
4383 node->articulationParameterCount = node->articulationParameterArray.n;
4386 node->_pduchange_es = node->_pduchange_es || es_info || es_force || es_deadreckoning || es_articulation ? TRUE : FALSE;
4387 if(shallow_compare_node_fields(X3D_NODE(node),node->_oldState,FIELDS_collision)){
4388 node->_pduchange_collision = TRUE;
4390 if(shallow_compare_node_fields(X3D_NODE(node),node->_oldState,FIELDS_events)){
4394 if(shallow_compare_node_fields(X3D_NODE(node),node->_oldState,FIELDS_fire)){
4395 node->_pduchange_fire = TRUE;
4397 if(shallow_compare_node_fields(X3D_NODE(node),node->_oldState,FIELDS_detonation)){
4398 node->_pduchange_detonation = TRUE;
4400 if(shallow_compare_node_fields(X3D_NODE(node),node->_oldState,FIELDS_munition)){
4401 node->_pduchange_fire = TRUE;
4402 node->_pduchange_detonation = TRUE;
4404 if(shallow_compare_node_fields(X3D_NODE(node),node->_oldState,FIELDS_rate)){
4405 node->_pduchange_fire = TRUE;
4406 node->_pduchange_detonation = TRUE;
4409 freeMallocedNodeFields(node->_oldState);
4410 shallow_copy_node(node->_oldState,X3D_NODE(node));
4417 if(node->isNetworkReader){
4418 if(node->_pduchange_em_info){
4419 mark_changed_node_fields(X3D_NODE(node), node->_oldState, FIELDS_em_info);
4421 if(node->_pduchange_create){
4422 if(node->addedEntities.n > 0)
4423 mark_changed_node_fields(X3D_NODE(node), node->_oldState, FIELDS_create);
4425 if(node->_pduchange_remove){
4426 if(node->removedEntities.n > 0)
4427 mark_changed_node_fields(X3D_NODE(node), node->_oldState, FIELDS_remove);
4429 reset_node_pduchanged(X3D_NODE(node));
4431 }
else if(node->isNetworkWriter){
4432 if(shallow_compare_node_fields(X3D_NODE(node),node->_oldState,FIELDS_em_info)){
4433 node->_pduchange_em_info = TRUE;
4435 if(shallow_compare_node_fields(X3D_NODE(node),node->_oldState,FIELDS_create)){
4436 if(node->addedEntities.n > 0)
4437 node->_pduchange_create = TRUE;
4439 if(shallow_compare_node_fields(X3D_NODE(node),node->_oldState,FIELDS_remove)){
4440 if(node->removedEntities.n > 0)
4441 node->_pduchange_remove = TRUE;
4444 freeMallocedNodeFields(node->_oldState);
4445 shallow_copy_node(node->_oldState,X3D_NODE(node));
4469void dead_reckon(
int drmethod,
double dtime,
float *p1,
float *R1xyza,
float *p0,
float *v0,
float *a0,
float *R0xyza,
float *RVxyza){
4490 veccopy4f(R1xyza,R0xyza);
4497 vecadd3f(p1,p0,vecscale3f(tmp,v0,dtime));
4498 veccopy4f(R1xyza,R0xyza);
4505 Quaternion qv, q1, q0;
4508 vecadd3f(p1,p0,vecscale3f(tmp,v0,dtime));
4511 vrmlrot4f_to_quaternion(&q0,R0xyza);
4512 vrmlrot_to_quaternion(&qv,RVxyza[0],RVxyza[1],RVxyza[2],RVxyza[3]*dtime);
4513 quaternion_multiply(&q1,&q0,&qv);
4514 quaternion_to_vrmlrot4f(&q1,R1xyza);
4522 float tmp3[3],tmp2[3],tmp1[3];
4523 Quaternion qv, q1, q0;
4525 vecadd3f(p1,p0,vecadd3f(tmp3,vecscale3f(tmp2,v0,dtime),vecscale3f(tmp1,a0,.5f*dtime*dtime)));
4528 vrmlrot4f_to_quaternion(&q0,R0xyza);
4529 vrmlrot_to_quaternion(&qv,RVxyza[0],RVxyza[1],RVxyza[2],RVxyza[3]*dtime);
4530 quaternion_multiply(&q1,&q0,&qv);
4531 quaternion_to_vrmlrot4f(&q1,R1xyza);
4542 float tmp3[3],tmp2[3],tmp1[3];
4543 vecadd3f(p1,p0,vecadd3f(tmp3,vecscale3f(tmp2,v0,dtime),vecscale3f(tmp1,a0,.5f*dtime*dtime)));
4545 veccopy4f(R1xyza,R0xyza);
4552 Quaternion qv, qa, q1, qbw;
4553 float deltap[3], att[3], tmp[3], tmp1[3], tmp2[3], tmp3[3];
4555 vrmlrot_to_quaternion(&qv,RVxyza[0],RVxyza[1],RVxyza[2],RVxyza[3]*dtime);
4556 vrmlrot4f_to_quaternion(&qbw,R0xyza);
4557 vecscale3f(tmp3,v0,dtime);
4558 quaternion_rotation3f(deltap,&qv,tmp3);
4559 quaternion_rotation3f(tmp2,&qbw,deltap);
4560 vecadd3f(p1,p0,tmp2);
4563 veccopy4f(R1xyza,R0xyza);
4569 Quaternion qv, qa, q1, qbw;
4570 float deltap[3], att[3], tmp[3], tmp1[3], tmp2[3], tmp3[3];
4572 vrmlrot_to_quaternion(&qv,RVxyza[0],RVxyza[1],RVxyza[2],RVxyza[3]*dtime);
4573 vrmlrot4f_to_quaternion(&qbw,R0xyza);
4574 vecscale3f(tmp3,v0,dtime);
4575 quaternion_rotation3f(deltap,&qv,tmp3);
4576 quaternion_rotation3f(tmp2,&qbw,deltap);
4577 vecadd3f(p1,p0,tmp2);
4581 quaternion_multiply(&q1,&qbw,&qv);
4582 quaternion_to_vrmlrot4f(&q1,R1xyza);
4599 Quaternion qv, qa, q1, qbw;
4600 float deltap[3], att[3], tmp[3], tmp1[3], tmp2[3], tmp3[3];
4602 vrmlrot_to_quaternion(&qv,RVxyza[0],RVxyza[1],RVxyza[2],RVxyza[3]*dtime);
4603 vrmlrot4f_to_quaternion(&qbw,R0xyza);
4604 vecadd3f(tmp3,vecscale3f(tmp2,v0,dtime),vecscale3f(tmp1,a0,.5f*dtime*dtime));
4605 quaternion_rotation3f(deltap,&qv,tmp3);
4606 quaternion_rotation3f(tmp2,&qbw,deltap);
4607 vecadd3f(p1,p0,tmp2);
4611 quaternion_multiply(&q1,&qbw,&qv);
4612 quaternion_to_vrmlrot4f(&q1,R1xyza);
4619 Quaternion qv, qa, q1, qbw;
4620 float deltap[3], att[3], tmp[3], tmp1[3], tmp2[3], tmp3[3];
4622 vrmlrot_to_quaternion(&qv,RVxyza[0],RVxyza[1],RVxyza[2],RVxyza[3]*dtime);
4623 vrmlrot4f_to_quaternion(&qbw,R0xyza);
4624 vecadd3f(tmp3,vecscale3f(tmp2,v0,dtime),vecscale3f(tmp1,a0,.5f*dtime*dtime));
4625 quaternion_rotation3f(deltap,&qv,tmp3);
4626 quaternion_rotation3f(tmp2,&qbw,deltap);
4627 vecadd3f(p1,p0,tmp2);
4630 veccopy4f(R1xyza,R0xyza);
4639 veccopy4f(R1xyza,R0xyza);
4644#define DRA_POS_THRSH 1.5
4645#define DRA_ORIENT_THRSH .175
4648 int withintol = TRUE;
4649 float p0[3], gap[3];
4651 veccopy3f(p0,node->_p0.c);
4653 vecdif3f(gap,p0,node->translation.c);
4654 if(veclength3f(gap) > DRA_POS_THRSH)
4660 if(node->isNetworkReader){
4661 if(node->_pduchange_es){
4662 mark_changed_node_fields(X3D_NODE(node), node->_oldState, FIELDS_es_transform);
4663 mark_changed_node_fields(X3D_NODE(node), node->_oldState, FIELDS_geo);
4665 }
else if(node->isNetworkWriter){
4666 if(shallow_compare_node_fields(X3D_NODE(node),node->_oldState,FIELDS_es_transform)
4667 || shallow_compare_node_fields(X3D_NODE(node),node->_oldState,FIELDS_geo)) {
4669 if(!transform_within_DeadReckoningTolerance1(node)) {
4670 node->_pduchange_es = TRUE;
4675 if(shallow_compare_node_fields(X3D_NODE(node),node->_oldState,FIELDS_es_transform)){
4680 node->__do_center = verify_translate ((GLfloat *)node->center.c);
4681 node->__do_trans = verify_translate ((GLfloat *)node->translation.c);
4682 node->__do_scale = verify_scale ((GLfloat *)node->scale.c);
4683 node->__do_rotation = verify_rotate ((GLfloat *)node->rotation.c);
4684 node->__do_scaleO = verify_rotate ((GLfloat *)node->scaleOrientation.c);
4686 node->__do_anything = (node->__do_center ||
4689 node->__do_rotation ||
4692 REINITIALIZE_SORTED_NODES_FIELD(node->children,node->_sortedChildren);
4698 compile_DIS_common(node);
4699 compile_EspduTransform1(node);
4700 compile_EspduTransform0(node);
4705 int drmethod, wasTransmitted;
4706 float p1[3],v1[3],a1[3], RVxyza[4], R0xyza[4], R1xyza[4];
4707 float p0[3],v0[3],a0[3];
4709 static int smoothing_frames = 230;
4710 static int want_smoothing = 1;
4713 wasTransmitted = FALSE;
4714 if(node->isNetworkReader){
4715 if(node->_lasttime == 0.0)
4717 if(node->_pduchange_es){
4719 node->_change_count++;
4720 wasTransmitted = TRUE;
4721 if(want_smoothing && node->_change_count){
4722 vecdif3f(node->_smoothingDelta.c,node->translation.c,node->_p0.c);
4723 node->_smoothingCount = smoothing_frames;
4724 if(node->_change_count > 1)
4725 node->_smoothingCount = 0;
4727 veccopy3f(node->_p0.c,node->translation.c);
4728 veccopy4f(node->_r0.c,node->rotation.c);
4730 veccopy3f(p0,node->_p0.c);
4732 veccopy3f(v0,node->linearVelocity.c);
4733 veccopy3f(a0,node->linearAcceleration.c);
4734 veccopy4f(RVxyza,node->_angularVelocity.c);
4735 veccopy4f(R0xyza,node->_r0.c);
4738 if(node->isStandAlone){
4739 veccopy3f(p0,node->translation.c);
4740 veccopy3f(v0,node->linearVelocity.c);
4741 veccopy3f(a0,node->linearAcceleration.c);
4742 veccopy4f(RVxyza,node->_angularVelocity.c);
4743 veccopy4f(R0xyza,node->rotation.c);
4745 if(node->isNetworkWriter){
4748 wasTransmitted = TRUE;
4749 node->_sent = FALSE;
4750 veccopy3f(node->_p0.c,node->translation.c);
4751 veccopy4f(node->_r0.c,node->rotation.c);
4753 veccopy3f(p0,node->_p0.c);
4754 veccopy3f(v0,node->linearVelocity.c);
4755 veccopy3f(a0,node->linearAcceleration.c);
4756 veccopy4f(RVxyza,node->_angularVelocity.c);
4757 veccopy4f(R0xyza,node->_r0.c);
4759 if(node->_lastframetime == 0.0)
4760 veccopy3f(node->_p0.c,p0);
4761 if(node->_lastframetime > 0.0){
4762 dtime = TickTime() - node->_lastframetime;
4763 drmethod = node->deadReckoning;
4766 dead_reckon(drmethod, dtime, p1, R1xyza, p0, v0, a0, R0xyza, RVxyza);
4767 veccopy3f(node->_p0.c,p1);
4768 veccopy4f(node->_r0.c,R1xyza);
4771 node->_lastframetime = TickTime();
4772 if(node->isNetworkReader){
4777 float psmooth[3], pzero[3], alpha;
4780 i = node->_smoothingCount;
4781 n = smoothing_frames;
4782 alpha = 1.0f - (float)min(i,n)/(float)n;
4783 vecset3f(pzero,0.0f,0.0f,0.0f);
4784 veclerp3f(psmooth,pzero,node->_smoothingDelta.c,alpha);
4785 vecdif3f(node->translation.c,node->_p0.c,psmooth);
4786 node->_smoothingCount++;
4788 veccopy3f(node->translation.c,node->_p0.c);
4790 veccopy4f(node->rotation.c,node->_r0.c);
4796 if(node->isNetworkReader) espdu_update_by_dead_reckoning(node);
4798 if(node->__geoSystem)
4799 geoprep(GEOSYS(node->__geoSystem),&node->geoCoords);
4800 if(!node->isNetworkReader) espdu_update_by_dead_reckoning(node);
4811 if(!renderstate()->render_vp) {
4813 push_transform_local_identity();
4815 if (node->__do_anything) {
4817 FW_GL_PUSH_MATRIX();
4818 FW_GL_PUSH_MATRIX();
4819 FW_GL_LOAD_IDENTITY();
4822 if (node->__do_trans)
4823 FW_GL_TRANSLATE_F(node->translation.c[0],node->translation.c[1],node->translation.c[2]);
4826 if (node->__do_center)
4827 FW_GL_TRANSLATE_F(node->center.c[0],node->center.c[1],node->center.c[2]);
4830 if (node->__do_rotation) {
4831 FW_GL_ROTATE_RADIANS(node->rotation.c[3], node->rotation.c[0],node->rotation.c[1],node->rotation.c[2]);
4835 if (node->__do_scaleO) {
4836 FW_GL_ROTATE_RADIANS(node->scaleOrientation.c[3], node->scaleOrientation.c[0], node->scaleOrientation.c[1],node->scaleOrientation.c[2]);
4841 if (node->__do_scale)
4842 FW_GL_SCALE_F(node->scale.c[0],node->scale.c[1],node->scale.c[2]);
4845 if (node->__do_scaleO)
4846 FW_GL_ROTATE_RADIANS(-node->scaleOrientation.c[3], node->scaleOrientation.c[0], node->scaleOrientation.c[1],node->scaleOrientation.c[2]);
4849 if (node->__do_center)
4850 FW_GL_TRANSLATE_F(-node->center.c[0],-node->center.c[1],-node->center.c[2]);
4855 FW_GL_GETDOUBLEV(GL_MODELVIEW_MATRIX,mat);
4857 FW_GL_TRANSFORM_D(mat);
4858 reset_transform_local(mat);
4870 if(!renderstate()->render_vp) {
4871 pop_transform_local();
4872 if (node->__do_anything) {
4877 if((node->_renderFlags & VF_Viewpoint) == VF_Viewpoint) {
4878 FW_GL_TRANSLATE_F(((node->center).c[0]),((node->center).c[1]),((node->center).c[2])
4880 FW_GL_ROTATE_RADIANS(((node->scaleOrientation).c[3]),((node->scaleOrientation).c[0]),((node->scaleOrientation).c[1]),((node->scaleOrientation).c[2])
4882 FW_GL_SCALE_F((
float)1.0/(((node->scale).c[0])),(
float)1.0/(((node->scale).c[1])),(
float)1.0/(((node->scale).c[2]))
4884 FW_GL_ROTATE_RADIANS(-(((node->scaleOrientation).c[3])),((node->scaleOrientation).c[0]),((node->scaleOrientation).c[1]),((node->scaleOrientation).c[2])
4886 FW_GL_ROTATE_RADIANS(-(((node->rotation).c[3])),((node->rotation).c[0]),((node->rotation).c[1]),((node->rotation).c[2])
4888 FW_GL_TRANSLATE_F(-(((node->center).c[0])),-(((node->center).c[1])),-(((node->center).c[2]))
4890 FW_GL_TRANSLATE_F(-(((node->translation).c[0])),-(((node->translation).c[1])),-(((node->translation).c[2]))
4894 if(node->__geoSystem)
4895 geofin(GEOSYS(node->__geoSystem),&node->geoCoords);
4902 if(!renderstate()->render_vp) {
4907 static int eventNumber = 0;
4908 if(node->eventNumber > eventNumber){
4909 node->firedTime = TickTime();
4910 eventNumber = node->eventNumber;
4912 double dtime = TickTime() - (double)node->munitionQuantity/(
double)max(1,node->firingRate) - node->firedTime ;
4913 if(dtime > 5.0)
return;
4914 mnode = (
struct X3D_EspduTransform*)dis_find_registered_node_by_entityid(node->munitionEntityID,TRUE,TRUE);
4916 for(i=0;i<node->munitionQuantity;i++){
4918 dtime = max(0.0,TickTime() - (
double)i/(
double)max(1,node->firingRate) - node->firedTime);
4919 dtime = min(5.0,dtime);
4920 float delta[3], velocity[3], progress[3], loc[3];
4921 vecdif3f(delta,node->munitionEndPoint.c,node->munitionStartPoint.c);
4922 vecscale3f(velocity,delta,1.0f/3.0f);
4923 vecscale3f(progress,velocity,(
float)dtime);
4924 if(veclength3f(progress) > veclength3f(delta)) {
4926 veccopy3f(loc,node->munitionEndPoint.c);
4927 if(i==(node->munitionQuantity-1)){
4928 node->fired1 = FALSE;
4929 node->detonateTime = TickTime();
4932 vecadd3f(loc,node->munitionStartPoint.c,progress);
4935 FW_GL_PUSH_MATRIX();
4936 FW_GL_TRANSLATE_F(loc[0],loc[1],loc[2]);
4941 normalChildren(mnode->children);
4952 if(!renderstate()->render_vp) {
4953 double dtime = TickTime() - node->detonateTime;
4954 if( dtime > 0.0 && dtime < .5){
4957 mnode = (
struct X3D_EspduTransform*)dis_find_registered_node_by_entityid(node->munitionEntityID,TRUE,TRUE);
4959 for(i=0;i<node->munitionQuantity;i++){
4960 float loc[3], fscale;
4962 veccopy3f(loc,node->detonationRelativeLocation.c);
4963 FW_GL_PUSH_MATRIX();
4964 FW_GL_TRANSLATE_F(loc[0],loc[1],loc[2]);
4965 fscale = dtime * 10.0f;
4966 FW_GL_SCALE_F(fscale,fscale,fscale);
4967 normalChildren(mnode->children);
4974void dis_register_collide(
struct X3D_Node* node,
double *transform);
4979 RETURN_FROM_CHILD_IF_NOT_FOR_ME
4984 double modelviewMatrix[16];
4985 FW_GL_GETDOUBLEV(GL_MODELVIEW_MATRIX, modelviewMatrix);
4986 dis_register_collide(X3D_NODE(node),modelviewMatrix);
4988 prep_sibAffectors((
struct X3D_Node*)node,&node->__sibAffectors);
4990 prep_BBox((
struct BBoxFields*)&node->bboxCenter);
4992 normalChildren(node->_sortedChildren);
4994 render_munitions(node);
4996 render_detonation(node);
5001 fin_sibAffectors((
struct X3D_Node*)node,&node->__sibAffectors);
5010 compile_TransmitterPdu0(node);
5015 compile_SignalPdu0(node);
5020 compile_ReceiverPdu0(node);
5026 geoprep(GEOSYS(node->__geoSystem),&node->geoCoords);
5028 geofin(GEOSYS(node->__geoSystem),&node->geoCoords);
5032 geoprep(GEOSYS(node->__geoSystem),&node->geoCoords);
5034 geofin(GEOSYS(node->__geoSystem),&node->geoCoords);
5038 geoprep(GEOSYS(node->__geoSystem),&node->geoCoords);
5040 geofin(GEOSYS(node->__geoSystem),&node->geoCoords);
5046 ConsoleMessage(
"domain %d category %d country %d kind %d extra %d subcat %d spec %d\n",
5047 anode->domain, anode->category,anode->country, anode->kind, anode->extra, anode->subcategory, anode->specific);
5051 compile_DISEntityManager0(node);
5054static int app_entity_last_id = 0;
5057 app_entity_last_id++;
5058 return app_entity_last_id;
5060#define GEOEL_WE_A (double)6378137
5068 static int ADD = 1, REMOVE = 2;
5071 if(node->addEntities.n){
5074 node->addedEntities.n = 0;
5075 for(j=0;j<node->addEntities.n;j++){
5076 int ibest,iscore,jscore;
5077 int entityID, applicationID, siteID;
5078 int port, multicastRelayPort;
5079 struct Uni_String *address, *networkMode, *multicastRelayHost;
5088 candi = node->addEntities.p[j];
5089 if(candi->_nodeType == NODE_DISEntityTypeMapping)
5100 if (mapping->n == 0) mapping = &node->children;
5101 for(i=0;i<mapping->n;i++){
5106 if (!bnode->kind || anode->kind == bnode->kind) jscore++;
5107 if (!bnode->domain || anode->domain == bnode->domain) jscore++;
5108 if (!bnode->country || anode->country == bnode->country) jscore++;
5109 if (!bnode->category || anode->category == bnode->category) jscore++;
5110 if (!bnode->subcategory || anode->subcategory == bnode->subcategory) jscore++;
5111 if (!bnode->specific || anode->specific == bnode->specific) jscore++;
5112 if (!bnode->extra || anode->extra == bnode->extra) jscore++;
5113 if(jscore > iscore){
5119 printf(
"\niscore %d ibest %d best.url %s\n", iscore, ibest, best->url.p[0]->strptr);
5121 applicationID = node->applicationID;
5122 siteID = node->siteID;
5123 entityID = newEntityID();
5124 address = node->address;
5126 networkMode = newASCIIString (
"networkWriter");
5127 multicastRelayHost = node->multicastRelayHost;
5128 multicastRelayPort = node->multicastRelayPort;
5131 }
else if(candi->_nodeType == NODE_EspduTransform) {
5136 for(i=0;i<node->mapping.n;i++){
5141 if (!bnode->kind || anode->entityKind == bnode->kind) jscore++;
5142 if (!bnode->domain || anode->entityDomain == bnode->domain) jscore++;
5143 if (!bnode->country || anode->entityCountry == bnode->country) jscore++;
5144 if (!bnode->category || anode->entityCategory == bnode->category) jscore++;
5145 if (!bnode->subcategory || anode->entitySubCategory == bnode->subcategory) jscore++;
5146 if (!bnode->specific || anode->entitySpecific == bnode->specific) jscore++;
5147 if(!bnode->extra || anode->entityExtra == bnode->extra) jscore++;
5148 if(jscore > iscore){
5160 applicationID = anode->applicationID;
5161 siteID = anode->siteID;
5162 entityID = anode->entityID;
5163 address = anode->address;
5165 networkMode = newASCIIString (
"networkReader");
5166 multicastRelayHost = anode->multicastRelayHost;
5167 multicastRelayPort = anode->multicastRelayPort;
5168 if(veclengthd(anode->geoCoords.c) < GEOEL_WE_A/2.0) use_GC = TRUE;
5179 iline = createNewX3DNode(NODE_Inline);
5181 iline->_parentResource = X3D_PROTO(node->_executionContext)->_parentResource;
5186 espdu = createNewX3DNode(NODE_EspduTransform);
5188 espdu->geoSystem.p[0] = newASCIIString(
"GC");
5189 espdu->geoSystem.n = 1;
5192 espdu->enabled = TRUE;
5193 espdu->isActive = TRUE;
5194 espdu->entityID = entityID;
5195 espdu->applicationID = applicationID;
5196 espdu->siteID = siteID;
5198 espdu->address = address;
5199 espdu->multicastRelayHost = multicastRelayHost;
5200 espdu->multicastRelayPort = multicastRelayPort;
5201 espdu->networkMode = networkMode;
5202 dis_set_node_lasttime(X3D_NODE(espdu),TickTime());
5209 dis_register(X3D_NODE(espdu),address->strptr,applicationID,entityID,multicastRelayHost->strptr,multicastRelayPort,
5210 networkMode->strptr,port,5.0,FALSE,siteID,5.0);
5213 add_node_to_broto_context(X3D_PROTO(node->_executionContext),X3D_NODE(iline));
5217 add_node_to_broto_context(X3D_PROTO(node->_executionContext),X3D_NODE(espdu));
5225 AddRemoveChildren(X3D_NODE(espdu), &espdu->children, (
struct X3D_Node * *)&iline, 1, ADD,__FILE__,__LINE__);
5227 shallow_copy_field(FIELDTYPE_MFString,(
union anyVrml*)&best->url,(
union anyVrml*)&iline->url);
5231 AddRemoveChildren(X3D_NODE(node), mfn, (
struct X3D_Node * *)&espdu, 1, ADD,__FILE__,__LINE__);
5233 AddRemoveChildren(X3D_NODE(node), &node->addedEntities, (
struct X3D_Node * *)&espdu, 1, ADD,__FILE__,__LINE__);
5238 if(node->addedEntities.n) MARK_EVENT(X3D_NODE(node),offsetof(
struct X3D_DISEntityManager,addedEntities));
5239 node->addEntities.n = 0;
5240 FREE_IF_NZ(node->addEntities.p);
5242 if(node->removeEntities.n){
5245 node->removedEntities.n = 0;
5246 for(j=0;j<node->removeEntities.n;j++){
5247 if(node->removeEntities.p[j]->_nodeType == NODE_DISEntityTypeMapping){
5248 int ibest,iscore,jscore;
5253 for(i=0;i<node->mapping.n;i++){
5254 if(node->mapping.p[i]->_nodeType == NODE_DISEntityTypeMapping){
5257 if(anode->domain == bnode->domain) jscore++;
5258 if(anode->category == bnode->category) jscore++;
5259 if(anode->country == bnode->country) jscore++;
5260 if(anode->kind == bnode->kind) jscore++;
5261 if(anode->extra == bnode->extra) jscore++;
5262 if(anode->subcategory == bnode->subcategory) jscore++;
5263 if(anode->specific == bnode->specific) jscore++;
5264 if(jscore > iscore){
5273 AddRemoveChildren(X3D_NODE(node), mfn, (
struct X3D_Node * *)&best, 1, REMOVE,__FILE__,__LINE__);
5275 AddRemoveChildren(X3D_NODE(node), &node->removedEntities, (
struct X3D_Node * *)&best->_child, 1, ADD,__FILE__,__LINE__);
5281 if(node->removedEntities.n) {
5285 node->removeEntities.n = 0;
5288Stack *dis_collide_stack = NULL;
5291 if(dis_collide_stack){
5292 for(i=0;i<dis_collide_stack->n;i++){
5295 double mvmInverse[16], m2m[16];
5298 usehit *uhit = vector_get_ptr(
usehit,dis_collide_stack,i);
5300 if(espdu->isNetworkReader)
continue;
5303 matinverseAFFINE(mvmInverse,uhit->mvm);
5304 extent6f_copy(ee,uhit->node->_extent);
5305 for(j=0;j<dis_collide_stack->n;j++){
5307 float eeb[6],eeba[6],eaXb[6];
5308 usehit *uhitb = vector_get_ptr(
usehit,dis_collide_stack,j);
5309 extent6f_copy(eeb,uhitb->node->_extent);
5310 if(extent6f_isSet(eeb)){
5312 matmultiplyAFFINE(m2m,mvmInverse,uhitb->mvm);
5314 extent6f_mattransform4d(eeba,eeb,m2m);
5316 extent6f_intersect_extent6f(eaXb,ee,eeba);
5317 if(extent6f_isSet(eaXb)){
5325 if(espdu->isCollided == FALSE){
5326 espdu->collideTime = TickTime();
5327 espdu->eventNumber = dis_next_event_number();
5329 espdu->collisionType = 33;
5330 espdu->isCollided = TRUE;
5331 espdu->eventSiteID = espdub->siteID;
5332 espdu->eventApplicationID = espdub->applicationID;
5333 espdu->eventEntityID = espdub->entityID;
5343 if(espdu->isCollided){
5344 espdu->isCollided = FALSE;
5345 espdu->collideTime = 0.0;
5346 espdu->collisionType = 0;
5347 espdu->eventSiteID = 0;
5348 espdu->eventApplicationID = 0;
5349 espdu->eventEntityID = 0;
5356void dis_clear_collide(){
5357 if(dis_collide_stack) dis_collide_stack->n = 0;
5359void dis_register_collide(
struct X3D_Node* node,
double *transform){
5362 if(!dis_collide_stack) dis_collide_stack = newStack(
usehit);
5364 for(i=0;i<dis_collide_stack->n;i++){
5365 usehit *uhit = vector_get_ptr(
usehit,dis_collide_stack,i);
5366 if(uhit->node == node){
5374 memcpy(uhit.mvm,transform,16*
sizeof(
double));
5375 uhit.userdata = NULL;
5376 vector_pushBack(
usehit,dis_collide_stack,uhit);
5380void dis_initialize() {
5383 static int once = 0;
5386 int sport = fwl_get_DISport() + fwl_get_testset();
5387 char* address = strdup(fwl_get_DISaddress());
5388 int site = fwl_get_DISsite();
5389 int application = fwl_get_DISapplication();
5393 if (!sockets_recv) sockets_recv = newVector(
struct dis_socket, 10);
5394 memset(&dsock, 0,
sizeof(
struct dis_socket));
5395 vector_pushBack(
struct dis_socket, sockets_recv, dsock);
5396 psock = vector_get_ptr(
struct dis_socket, sockets_recv, sockets_recv->n - 1);
5397 psock->address = address;
5398 psock->port = sport;
5399 psock->multicastRelayHost = strdup(
"");
5400 psock->multicastRelayPort = 0;
5403 dis_open_socket(psock);
5406 if (!sockets_send) sockets_send = newVector(
struct dis_socket, 10);
5407 memset(&dsock, 0,
sizeof(
struct dis_socket));
5408 vector_pushBack(
struct dis_socket, sockets_send, dsock);
5409 psock = vector_get_ptr(
struct dis_socket, sockets_send, sockets_send->n - 1);
5410 psock->address = address;
5411 psock->port = sport;
5412 psock->multicastRelayHost = strdup(
"");
5413 psock->multicastRelayPort = 0;
5416 dis_open_socket(psock);
5434void dis_initialize() {}
5437void fwl_sendreceive_DIS(){
5470 dis_clear_collide();
unsigned short quantity
how many of the munition were fired
unsigned short rate
rate at which the munition was fired
struct EntityType munition
What munition was used in the burst.
unsigned short warhead
type of warhead
unsigned short fuse
type of fuse used
unsigned char collisionType
ID of event.
struct EventID eventID
ID of event.
struct EntityID issuingEntityID
ID of the entity that issued the collision PDU.
struct EntityID collidingEntityID
ID of entity that has collided with the issuing entity ID.
struct Vector3Float entityAngularVelocity
angular velocity of the entity
struct Vector3Float entityLinearAcceleration
Linear acceleration of the entity.
unsigned char deadReckoningAlgorithm
enumeration of what dead reckoning algorighm to use
char otherParameters[15]
other parameters to use in the dead reckoning algorithm
struct Vector3Float locationInEntityCoordinates
location of the detonation or impact in the target entity's coordinate system.
struct BurstDescriptor burstDescriptor
Describes munition used.
struct Vector3Double locationInWorldCoordinates
where the detonation is, in world coordinates
unsigned char detonationResult
result of the explosion
struct EventID eventID
ID firing event.
unsigned char numberOfArticulationParameters
How many articulation parameters we have.
struct EntityID munitionID
ID of muntion that was fired.
struct Vector3Float velocity
ID firing event.
unsigned short application
The application ID.
unsigned short site
The site ID.
unsigned short entity
the entity ID
struct Vector3Float entityLinearVelocity
Describes the speed of the entity in the world.
struct EntityType entityType
Describes the type of entity in the world.
struct EntityID entityID
Unique ID for an entity that is tied to this state information.
void * articulationParameters
variable length list of articulation parameters
struct Orientation entityOrientation
describes the orientation of the entity, in euler angles
int entityAppearance
a series of bit flags that are used to help draw the entity, such as smoking, on fire,...
struct Vector3Double entityLocation
describes the location of the entity in the world
struct DeadReckoningParameter deadReckoningParameters
parameters used for dead reckoning
char numberOfArticulationParameters
How many articulation parameters are in the variable length list.
unsigned char domain
Domain of entity (air, surface, subsurface, space, etc)
unsigned short country
country to which the design of the entity is attributed
unsigned char entityKind
Kind of entity.
unsigned char specific
specific info based on subcategory field
unsigned char category
category of entity
unsigned char subcategory
subcategory of entity
unsigned short application
The application ID.
unsigned short eventNumber
the number of the event
unsigned short site
The site ID.
struct BurstDescriptor burstDescriptor
Describes munitions used in the firing event.
struct EntityID munitionID
ID of the munition that is being shot.
struct Vector3Float velocity
Velocity of the ammunition.
struct EventID eventID
ID of event.
struct Vector3Double locationInWorldCoordinates
location of the firing event
float range
range to the target
unsigned short system
system
unsigned short detail
detail
unsigned short spreadSpectrum
spread spectrum, 16 bit boolean array
unsigned short major
major
unsigned char pduType
Type of pdu, unique for each PDU class.
short padding
zero-filled array of padding
unsigned short radioId
particular radio within an entity
struct EntityID entityId
ID of the entitythat is the source of the communication.
unsigned short country
country to which the design of the entity is attributed
unsigned char domain
Domain of entity (air, surface, subsurface, space, etc)
unsigned char entityKind
Kind of entity.
unsigned char category
category of entity
unsigned char nomenclatureVersion
specific info based on subcategory field
struct EntityID transmitterEntityId
ID of transmitter.
unsigned short transmitterRadioId
ID of transmitting radio.
unsigned short receiverState
encoding scheme used, and enumeration
float receivedPoser
received power
void * data
list of eight bit values
unsigned int sampleRate
sample rate
unsigned short encodingScheme
encoding scheme used, and enumeration
short dataLength
length od data
unsigned short tdlType
tdl type
short samples
number of samples
struct EntityID originatingEntityID
Entity that is sending message.
unsigned char modulationParameterCount
how many modulation parameters we have
unsigned short antennaPatternType
antenna pattern type
unsigned char transmitState
transmit state
float power
transmission power
unsigned short cryptoSystem
crypto system enumeration
float transmitFrequencyBandwidth
transmit frequency Bandwidth
struct RadioEntityType radioEntityType
linear accelleration of entity
struct Vector3Double antennaLocation
Location of antenna.
struct Vector3Float relativeAntennaLocation
relative location of antenna
unsigned short cryptoKeyId
crypto system key identifer
unsigned char inputSource
input source
unsigned short antennaPatternCount
atenna pattern length
unsigned long long frequency
frequency
struct ModulationType modulationType
modulation
unsigned int variableDatumID
ID of the variable datum.
void * variableDatums
variable length list of 64-bit datums
unsigned int variableDatumLength
length of the variable datums
struct EntityID firingEntityID
ID of the entity that shot.