FreeWRL / FreeX3D 4.3.0
Component_DIS.c
1
2/****************************************************************************
3 This file is part of the FreeWRL/FreeX3D Distribution.
4
5 Copyright 2009 CRC Canada. (http://www.crc.gc.ca)
6
7 FreeWRL/FreeX3D is free software: you can redistribute it and/or modify
8 it under the terms of the GNU Lesser Public License as published by
9 the Free Software Foundation, either version 3 of the License, or
10 (at your option) any later version.
11
12 FreeWRL/FreeX3D is distributed in the hope that it will be useful,
13 but WITHOUT ANY WARRANTY; without even the implied warranty of
14 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 GNU General Public License for more details.
16
17 You should have received a copy of the GNU General Public License
18 along with FreeWRL/FreeX3D. If not, see <http://www.gnu.org/licenses/>.
19****************************************************************************/
20
21
22/*******************************************************************
23
24 X3D DIS Component
25
26*********************************************************************/
27
28
29#include <config.h>
30#include <system.h>
31#include <display.h>
32#include <internal.h>
33#include <iglobal.h>
34#include <libFreeWRL.h>
35
36#include "../vrml_parser/Structs.h"
37#include "../vrml_parser/CRoutes.h"
38#include "../main/headers.h"
39
40#include "../world_script/fieldSet.h"
41#include "../x3d_parser/Bindable.h"
42#include "Collision.h"
43#include "quaternion.h"
44#include "Viewer.h"
45#include "../opengl/Frustum.h"
46#include "../opengl/Material.h"
47#include "../opengl/OpenGL_Utils.h"
48#include "../input/EAIHelpers.h" /* for newASCIIString() */
49
50#include "Polyrep.h"
51#include "LinearAlgebra.h"
52#include "Children.h"
53#include "Vector.h"
54#include "Component_Geospatial.h"
55#include "Component_DIS.h"
56#include "Component_Grouping.h"
57#include "RenderFuncs.h"
58
59//from CparseParser
60void add_node_to_broto_context(struct X3D_Proto *currentContext,struct X3D_Node *node);
61
62#ifndef WIN32
63#define SOCKET int
64#include <sys/socket.h>
65#include <netinet/in.h>
66#include <arpa/inet.h>
67#endif
68/*
69typedef struct pComponent_DIS{
70 int something;
71}* ppComponent_DIS;
72void *Component_DIS_constructor(){
73 void *v = MALLOCV(sizeof(struct pComponent_DIS));
74 memset(v,0,sizeof(struct pComponent_DIS));
75 return v;
76}
77void Component_DIS_init(struct tComponent_DIS *t){
78 //public
79 //private
80 t->prv = Component_DIS_constructor();
81 {
82 ppComponent_DIS p = (ppComponent_DIS)t->prv;
83 p->something = 0;
84 }
85}
86void Component_DIS_clear(struct tComponent_DIS *t){
87 //public
88}
89
90References:
91http://www.web3d.org/documents/specifications/19775-1/V3.3/Part01/components/dis.html
92https://github.com/open-dis/open-dis-cpp
93http://www.web3d.org/x3d/content/examples/Basic/DistributedInteractiveSimulation/
94http://x3dgraphics.com/slidesets/X3dForAdvancedModeling/DistributedInteractiveSimulation.pdf
95-- brutzman slideshow on DIS
96https://en.wikipedia.org/wiki/Distributed_Interactive_Simulation
97http://open-dis.sourceforge.net/Open-DIS.html
98http://movesinstitute.org/~mcgredo/MV3500/hla/1278.1-200X%20Draft%2016%20rev%2018.pdf
99- 2012 DIS draft
100- p.332 7.2.2 espdu struct/contents
101- p.665 Annex E dead reckoning formula
102
103Don's references:
104a. IITSEC 2017 slideset, DIS 101
105 https://gitlab.nps.edu/Savage/NetworkedGraphicsMV3500/raw/master/presentations/IITSEC2018_DIS_Tutorial.pptx
106
107b. X3D and Distributed Interactive Simulation (DIS)
108 http://x3dgraphics.com/slidesets/X3dForAdvancedModeling/DistributedInteractiveSimulation.pdf
109 (brutzman slides)
110
111c. X3D v3.3 Distributed interactive simulation (DIS) component
112 http://www.web3d.org/documents/specifications/19775-1/V3.3/Part01/components/dis.html
113
114d. IEEE Standards Maintained by SISO SAC
115 https://www.sisostds.org/ProductsPublications/Standards/IEEEStandards.aspx
116
117
118Problem: our C .h and the DIS.lib (cpp) .h clash, very messy
119x didn't find a combination of headers that worked
120Options:
1211. clean up our headers
1222. convert DIS.lib objects we need to flat C structs
123 - about 30 structs
124 a) manually, from .cpp
125 b) hack xmlpg https://github.com/open-dis/xmlpg CppGenerator.java
126 into a CGenerator.java and generate flat C
1273. wrap DIS objects -just ones we need- in flat C interfaces (about 30)
1284. somehow show cpp just the C structs it needs, like X3D_EspduTransform
129 - about 5 x3d structs
130Choice: option 2.b hack xmlpg CGenerator.java DONE
131- benefits: easy to interface, could do just .h (no lib), code & license is ours/freewrl
132-disadvantages: someone has to do hacking upstream in CGenerator.java, and
133 duplicate all the CppUtils (that wrap the pdu classes) in C,
134 and mistakes can happen during transcription (risk)
135
136Problem: Transforms - unclear how goecoords are to be used.
137The DIS specs have 5 terms:
138global, world, local, body, entity
139And there are 2 major geospatial states:
140a) geoCoords not used
141- geoCoords = 0,0,0 (default)
142b) geoCoords used
143
144Proposed use of terms:
145global == world = gc ^see World Coordinates below
146local - used only for some Dead Reckoning formulas
147 == TCS (topocentric coord sys) from geospatial component, except with axes re-arranged/swizzled
148 Draft DIS specs p.675:
149 "The local coordinate system used here is defined by North, East, and Down axes
150 with their origin at the entity's center of bounding volume."
151entity == body - the coordinate system for espdu.transform.children
152 -except DIS swizzles so Z is down, X forward, see Gimbals.x3d test scene
153World2body = Location,(phi,theta,psi)
154in web3d we can split world2body in 2 parts, to make dead-reckoning in local coords easier:
155- World2Local x Local2body
156- where:
157-- World2local is the gc2tcs from geospatial
158-- local2body is the rest of world2body: local2body = world2local.inverse x world2body
159if a) no geoCoords used, then
160- World2local == 0
161and
162- world2body == local2body (we do it all with transform.translation,rotation)
163Freewrl strategy:
164- we will convert the DIS coordinate axes and naming to web3d conventions during node2pdu and pdu2node
165 - convert from geocentric GC to Topocentric TCS if geocoords != 000
166- we will be working in web3d coordinate systems elsehwere, including in dead reckoning
167 (so no need to swizzle axes if our dead reckonging formulas are all in web3d conventions)
168Q. is this compatible with Xj3d convention?
169A. don't know. But Gimbal.x3d is showing (what I interpret as)
170 the Dead Reckoning Local coord system as being
171 swizzled and aligned with what looks like our geospatial TCS
172 That could indicate where NPS thinking was on how X3D relates to DIS.
173 So its a good bet espduTransform.(translation.rotation) are tcs2body aka local2body.
174
175
176World Coordinates:
177The Entity State PDU Location field is 64bit wgs84 geocentric coordinates.
178In the draft 2012 specs document,
179p.3 1.6.3.1 World coordinate system WGS84, meters.
180p.4 Figure 1 shows world coordinates.
181- they look exactly like GC.
182- Z through Northpole
183- X through prime meridian
184- right handed (Y through bangladesh)
185p.48 e) 1) Location with respect to the world
186p.330 6.2.98 World Coordinates record
187Location of the origin of the entity's or object's coordinate system, target locations,
188 detonation locations, and other points shall be specified by a set of three coordinates: X, Y, and Z,
189 represented by 64-bit floating point numbers. The world coordinate system shall be as specified in 1.6.3.
190 The format of the World Coordinates record shall be as shown in Table134.
191p.333
192h) Entity Location. This field shall specify an entity’s physical location in the simulated world,
193 and shall be represented by a World Coordinates record (see 6.2.98).
194p.334 Entity State PDU table 135
195/World Coordinates
196
197
198*/
199//#define WITH_DIS 1
200#ifdef WITH_DIS
201#include "../DIS/DIS.h"
202
203//there's another .pdf with enums
204// SISO-REF-010-2015 Enumerations for Simulation Interoperability V21 20150413.pdf
205// we'll do just a few here as needed
206// SM > DataRecord > datumType
207enum UID66 {
208Kind = 11110,
209Domain = 11120,
210Country = 11130,
211Category = 11140,
212Subcategory = 11150,
213Specific = 11160,
214Extra = 11170,
215/*
21631000 Position
217 31010 Route (Waypoint) type
218 31100 MilGrid10
219 31200 Geocentric Coordinates
220 31210 X
221 31220 Y
222 31230 Z
223 31300 Latitude
224 31400 Longitude
225 ...
226 31600 Altitude
227 */
228};
229
230#endif //WITH_DIS
231
232
233
234
235static int allow_DIS = 0;
236static char* DISaddress = NULL;
237static int DISport = 0;
238static int DISsite = 0;
239static int DISapplication = 0;
240void fwl_init_DIS(){
241 //from commandline --DIS or -D
242 allow_DIS = 1;
243}
244void fwl_set_DISaddress(char* address) {
245 DISaddress = address;
246}
247void fwl_set_DISport(int port) {
248 DISport = port;
249}
250void fwl_set_DISsite(int site) {
251 DISsite = site;
252}
253void fwl_set_DISapplication(int app) {
254 DISapplication = app;
255}
256static int dis_verbose = FALSE; // TRUE; //just for a receiver, not for sender
257void fwl_set_DISverbose(int verbose) {
258 dis_verbose = verbose;
259}
260int disverbose() {
261 return dis_verbose;
262}
263int fwl_get_allow_DIS(){
264 return allow_DIS;
265}
266char* fwl_get_DISaddress() {
267 return DISaddress;
268}
269int fwl_get_DISport() {
270 return DISport;
271}
272int fwl_get_DISsite() {
273 return DISsite;
274}
275int fwl_get_DISapplication() {
276 return DISapplication;
277}
278void fwl_set_allow_DIS(int allow){
279 allow_DIS = allow ? 1 : 0;
280}
281//testset is an int added to port when regression tessting DIS sets
282// so 2 sets of mulitple freewrl instances don't mix their communications
283static int testset = 0;
284void fwl_set_testset(int iset) {
285 testset = iset;
286}
287int fwl_get_testset() {
288 return testset;
289}
290
291#ifdef WITH_DIS
292
293/*
294DIS - Distributed Interactive Simulation communication
295Concepts as understood by dug9 Oct 23, 2017
296
297IMPLIED SHARED RECEIVER LOOP
298- loop 1:1 socket(IP,port)
299- 1st node to declare a port opens it and joins
300- other nodes declaring same port join
301- reading is at fine time granularity, and times are added up
302- multiple receives on one loop are all read to flush and take pdu with latest timestamp
303- if received too soon, packets dropped (and dead reconning used, or radio signal is choppy)
304- timestamp is kept as start of interval, and each loop increments it
305- on recv packet, it loops over the packet unmarshalling multiple pdus
306 - it loops over nodes registered on that port to find a matching entityID/entityId
307 - if a match, updates entity
308 - if no match, discards/skips
309
310IMPLIED SHARED SENDER LOOP
311- loop 1:1 socket(IP,port)
312- 1st node to declare a port opens it and joins
313- other nodes declaring same port join
314- nodes can have different send intervals
315- fine-granularity loop increments time and checks who is ready to send
316- bundles pdus that are ready to send at the same time
317
318Design Options
319A. per-frame - iterate over all send and receive sockets once per frame,
320 -- in the render thread
321B. per-socket-direction thread
322
323Major Issues:
3241. change detection
325- for scripts and protos we have special field structs with a change flag;
326 we do that because scripts and protos are 'opaque' to routing algorithms
327- DIS nodes when receiving are changing fields like a script node might change its fields, opaque to routing
328 and for script nodes we iterate over fields after running a script, to check fields for change flag and route
329- for sending, somewhat analogously when we change a field via routing on a DIS node,
330 we need to let the pdu sending code know which puds changed, so it can send just the changed ones
331- options:
332 a) implement nodes in terms of i) script or ii) proto fields
333 - don't have an example of script or proto used as builtin
334 - its the parser that generates them from scene file info
335 ? would that mean a lot of changes to perl code generator and parser code?
336 - might be helpful to harmonize all builtin, proto, script and shader nodes to have same field struct
337 x but will take a massive refactoring effort to do it
338 - and with DIS, you send/receive whole pdus, and maybe only one little thing changed,
339 per-node-field flags wouldn't help because those flags aren't transmitted/received with pdu
340 b) pre/post value comparison ie _oldvalue
341 - lots of examples of this, but not on such big nodes
342 choice: b) SFNode node->_oldState copies entire node
343 - generic functions compare old new fields to detect changes
344 - but keep option a) in mind for future
345
3462. nodes have a lot of similar fields, resulting in duplicate code
347x and freewrl has no structs for 'abstract interface'
348options:
349a) giant macros - used throughout freewrl for this reason
350b) careful ordering of fields so common fields are first, and nodes can be cast to a common type
351c) some kind of abstract interface added to code generation system
352 - maybe in the future
353d) change to OO language and use inheritance and polymorphism
354 - maybe in the future
355e) functions with switch-case on nodetype
356For now in DIS we're going to use b) for espduTransform and 3 radio nodes and entityManager and e)
357
358*/
359
360
361// http://movesinstitute.org/~mcgredo/MV3500/hla/enum99_2.pdf
362// p.6
363// 62 and 65 Comment-R seem duplicates, so we set it to Comment-R2
364
365enum PDUType
366{
367 PDU_OTHER = 0,
368 PDU_ENTITY_STATE = 1,
369 PDU_FIRE = 2,
370 PDU_DETONATION = 3,
371 PDU_COLLISION = 4,
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,
385 PDU_DATA_QUERY = 18,
386 PDU_SET_DATA = 19,
387 PDU_DATA = 20,
388 PDU_EVENT_REPORT = 21,
389 PDU_COMMENT = 22,
390 PDU_ELECTROMAGNETIC_EMISSION = 23,
391 PDU_DESIGNATOR = 24,
392 PDU_TRANSMITTER = 25,
393 PDU_SIGNAL = 26,
394 PDU_RECEIVER = 27,
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,
401 PDU_ISGROUPOF = 34,
402 PDU_TRANSFER_CONTROL = 35,
403 PDU_ISPARTOF_= 36,
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,
413 PDU_TSPI = 46,
414 PDU_APPEARANCE = 47,
415 PDU_ARTICULATED_PARTS = 48,
416 PDU_LE_FIRE = 49,
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,
426 PDU_SET_DATA_R = 59,
427 PDU_DATA_R = 60,
428 PDU_EVENT_REPORT_R = 61,
429 PDU_COMMENT_R = 62, //62
430 PDU_RECORD_QUERY_R = 63,
431 PDU_SET_RECORD_R = 64,
432 PDU_COMMENT_R2 = 65, //DUPLICATE OF 62
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,
442};
443
444void axisangle2ypr(float *xyza, float *ypr)
445{
446 //y = yaw = azimuth
447 //p = pitch = elevation
448 //r = roll
449 //assumes z is up, you re-arrange your inputs if other
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);
454 if(flen == 0.0f){
455 yaw = 0.0f;
456 pitch = acos(-1.0) * .5; //90
457 }else{
458 yaw = atan2(y,x);
459 pitch = atan(z/flen);
460 }
461 roll = a;
462 ypr[0] = yaw;
463 ypr[1] = pitch;
464 ypr[2] = roll;
465}
466void ypr2axisangle(float *ypr, float *xyza)
467{
468 //y = yaw = azimuth
469 //p = pitch = elevation
470 //r = roll
471 //assumes z is up, you re-arrange your inputs if other
472 float yaw, pitch, roll, x,y,z,a, xyz[3];
473 yaw = ypr[0];
474 pitch = ypr[1];
475 roll = ypr[2];
476 a = roll;
477 x = cos(pitch)*cos(yaw);
478 y = cos(pitch)*sin(yaw);
479 z = sin(pitch); //or sqrt(1.0 - (x*x + y*y))
480 xyz[0] = x;
481 xyz[1] = y;
482 xyz[2] = z;
483 vecnormalize3f(xyza,xyz);
484 xyza[3] = a;
485}
486//dis stores vectors in structs .xyz, we do float[3], conversions:
487struct Vector3Float *vec3f2vector3float(struct Vector3Float *b, float *a){
488 b->x = a[0];
489 b->y = a[1];
490 b->z = a[2];
491 return b;
492}
493float *vector3float2vec3f(float *b,struct Vector3Float *a){
494 b[0] = a->x;
495 b[1] = a->y;
496 b[2] = a->z;
497 return b;
498}
499struct Vector3Double *vec3d2vector3double(struct Vector3Double *b, double *a){
500 b->x = a[0];
501 b->y = a[1];
502 b->z = a[2];
503 return b;
504}
505double *vector3double2vec3d(double *b,struct Vector3Double *a){
506 b[0] = a->x;
507 b[1] = a->y;
508 b[2] = a->z;
509 return b;
510}
511// freewrl's once-per-frame timestamp is called TickTime
512// - and TickTime is a double value representing seconds since 1970, including fractions of a second
513// DIS Clock Time record is a 64bit consisting of
514// 32bit Hours since 1970 UTC and
515// 32bit Timestamp fraction-past-the-hour scaled by 2**31
516// - the least significant bit- is reserved for flagging 1=AbsoluteTime (vs relative =0)
517// in draft specs see
518// 6.2.88 TimeStamp p.319
519// G.4 Time Terminology p.686
520//
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;
528 bitmask = 0;
529 bitmask = ~bitmask;
530 if(!iabs)
531 bitmask = bitmask << 1; //clear least significant bit
532 *hourfraction = *hourfraction & bitmask;
533 if(iabs) *hourfraction |= 1;
534}
535double DISTime2TickTime(unsigned int hours, unsigned int hourfraction){
536 //pass in 0 for hours if relative timestamp (then we'll take hours from TickTime())
537 double fraction, mantissa;
538 int iabs;
539 unsigned int bitmask;
540 bitmask = 1;
541 iabs = (hourfraction & bitmask) != 0 ? TRUE : FALSE;
542 hourfraction = hourfraction >> 1; //take everything except least significant bit
543 fraction = hourfraction;
544 fraction /= pow(2.0,31.0);
545 if(iabs)
546 mantissa = hours;
547 else
548 mantissa = floor(TickTime() / 3600.0);
549 mantissa += fraction;
550 mantissa *= 3600.0;
551 return mantissa;
552
553}
554
555//A. per-frame
556
557struct dis_socket {
558 int port;
559 char *address;
560 SOCKET socket;
561 struct sockaddr_in saddr;
562 int multicastRelayPort;
563 char *multicastRelayHost;
564 int idir; //0 = receive, 1 = send
565 struct Vector *registered;
566 double lasttime;
567};
568
569
570void print_stream(unsigned char *buf, int nbytes){
571 int i,j;
572 for(i=0;i<min(210,nbytes);i+=10){
573 int j;
574 printf("%d\t",i);
575 for(j=0;j<10;j++){
576 printf("%5d",(int)buf[i+j]);
577 }
578 printf("\n");
579 }
580}
581//TCS - geospatial topocentric coordinate system
582//local - DIS equivalent
583double *tcs2localswizzled(double *local,double *tcs){
584 local[0] = -tcs[2]; //north
585 local[1] = tcs[0]; //east
586 local[2] = -tcs[1]; //vertical
587 return local;
588}
589double *local2tcsswizzled(double *tcs,double *local){
590 tcs[0] = local[1]; //east
591 tcs[1] = -local[2]; //vertical
592 tcs[2] = -local[0]; //north
593 return tcs;
594}
595void node2pdu_entityType(int *entityKind, struct EntityType *entityType){
596 //assumes node field order: kind, domain, country, category, subcategory, specific, extra
597 //p.262 draft standard http://movesinstitute.org/~mcgredo/MV3500/hla/1278.1-200X%20Draft%2016%20rev%2018.pdf
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];
605}
606void pdu2node_entityType( struct EntityType *entityType, int *entityKind){
607 //assumes node field order: kind, domain, country, category, subcategory, specific, extra
608 //p.262 draft standard http://movesinstitute.org/~mcgredo/MV3500/hla/1278.1-200X%20Draft%2016%20rev%2018.pdf
609 entityKind[0] = entityType->entityKind;
610 entityKind[1] = entityType->domain;
611 entityKind[2] = entityType->country;
612 entityKind[3] = entityType->category;
613 entityKind[4] = entityType->subcategory;
614 entityKind[5] = entityType->specific;
615 entityKind[6] = entityType->extra;
616}
617static int dis_event_number = 0;
618int dis_next_event_number(){
619 dis_event_number++;
620 return dis_event_number;
621}
622static int dis_fire_mission_index = 0;
623int dis_next_fire_mission_index(){
624 dis_fire_mission_index++;
625 return dis_fire_mission_index;
626}
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){
629 //http://www.web3d.org/documents/specifications/19775-1/V3.3/Part01/components/dis.html#EspduTransform
630 //EspuTransform integrates the following pdus:
631 //EntityStatePDU, CollisionPDU, DetonationPDU, FirePDU, CreateEntity, and RemoveEntity.
632 //http://movesinstitute.org/~mcgredo/MV3500/hla/1278.1-200X%20Draft%2016%20rev%2018.pdf
633 //p.332 7.2.2 espdu struct/contents
634 //Q. how do create/remove work?
635 struct Vector *pdus;
636 struct X3D_EspduTransform * pnode = (struct X3D_EspduTransform*)node;
637 pdus = newVector(struct Pdu *, 6);
638
639 //ENTITYSTATE
640 //if(pnode->_pduchange_es_articulation || pnode->_pduchange_es_deadreckoning || pnode->_pduchange_es_info || pnode->_pduchange_es_force){
641 if(0) printf("es pduchange %d heartbeat %d\n",pnode->_pduchange_es,isHeartbeat);
642 if(pnode->_pduchange_es || isHeartbeat){
643 float xyz[3];
644 struct EntityStatePdu *espdu;
645 espdu = (struct EntityStatePdu*)dis_ctor(type_EntityStatePdu);
646 //entity
647 espdu->entityType.category = 1; //not 77
648 espdu->entityID.entity = pnode->entityID;
649 espdu->entityID.application = pnode->applicationID;
650 espdu->entityID.site = pnode->siteID;
651 //translation - assumes companion scenes will have same parent transform stack
652 //(x, -z, y).
653 if(pnode->__geoSystem){
654 //a default geo scene is in TCS at Accra (Grenwich & equator)
655 Quaternion qgc2tcs, qtcs2body, qgc2body;
656 struct SFVec3d gd, gc, translate;
657 struct SFVec4d rotate;
658 double localxyz[3], tcsxyz[3], tcs2bodyxyz[3], world2bodyxyz[3];
659 float xyza[4];
660 Geosys *gs;
661 gs = GEOSYS(pnode->__geoSystem);
662 user2gc(gs,&pnode->geoCoords,1,&gc);
663 //gc2gd(gs,&gc,1,&gd);
664 //gc2tcs_transform(gs,&gd,&translate,&rotate);
665 gc2tcsB_transform(gs,&gc,&translate,&rotate);
666 //somehow get body/entity into world/gc - rotation and translation
667 {
668 //rotation
669 float ypr[3];
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);
674 //vecprint4fb("send xyza",xyza,"\n");
675 xyza[3] = -xyza[3];
676 axisangle2ypr(xyza,ypr);
677
678 espdu->entityOrientation.psi = -ypr[0]; //gimbal.js shows -yaw
679 espdu->entityOrientation.theta = ypr[1];
680 espdu->entityOrientation.phi = ypr[2];
681
682 }
683 {
684 //translation
685 struct SFVec3d tcs, world;
686 float2double(tcs.c,pnode->translation.c,3);
687 //tcs2gc(gs,&gd,&tcs,1,&world);
688 tcs2gcB(gs,&gc,&tcs,1,&world);
689 vec3d2vector3double(&espdu->entityLocation,world.c);
690 }
691
692 //send > geo > dead reckoning
693 if(pnode->deadReckoning < 6){
694 //first update linear V,A, angularV
695 //for drmethod < 6
696 //all of which are in Local/TCS for freewrl/web3d, instead of world for DIS
697 pnode->_change_count++;
698 if(pnode->_change_count > 1){
699 double dtime;
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);
703 //pnode->_change_count = min(pnode->_change_count,2);
704 if(pnode->_change_count > 2){
705 //a = (v1-v0)/dt
706 vecscale3f(a1,vecdif3f(tmp,v1,pnode->linearVelocity.c),1.0f/dtime);
707 veccopy3f(pnode->linearAcceleration.c,a1);
708 //v1 = v0 - 1/2at**2
709
710 }else{
711 vecset3f(pnode->linearAcceleration.c,0.0,0.0,0.0);
712 }
713 veccopy3f(pnode->linearVelocity.c,v1);
714 {
715 //update angular velocity
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;
723 }
724 }
725 }
726 veccopy3f(pnode->_lastp0.c,pnode->translation.c);
727 veccopy4f(pnode->_lastr0.c,pnode->rotation.c);
728 pnode->_lastp0time = TickTime();
729
730 espdu->deadReckoningParameters.deadReckoningAlgorithm = pnode->deadReckoning;
731 {
732 float V[3], A[3];
733 // in TCS aka Local
734 veccopy3f(V,pnode->linearVelocity.c);
735 veccopy3f(A,pnode->linearAcceleration.c);
736
737 //convert Local/TCS linear/angular V,A to world or to Entity, depending on DR parameter
738 //http://movesinstitute.org/~mcgredo/MV3500/hla/1278.1-200X%20Draft%2016%20rev%2018.pdf
739 //p.333, p.329
740 if(pnode->deadReckoning < 6){
741 //convert our TCS/Local to world
742 //Vgc = tcs2gc x Vtcs
743 Quaternion q;
744 vrmlrot4d_to_quaternion(&q,rotate.c);
745 quaternion_inverse(&q,&q);
746 quaternion_rotation3f(V,&q,V);
747 quaternion_rotation3f(A,&q,A);
748 } else {
749 if(0){
750 //convert our TCS/Local to entity
751 //Vbody = tcs2body x Vtcs
752 Quaternion q;
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);
757 }else{
758 //keep entity in entity
759 }
760 }
761 vec3f2vector3float(&espdu->entityLinearVelocity,V);
762 vec3f2vector3float(&espdu->deadReckoningParameters.entityLinearAcceleration,A);
763
764 }
765 {
766 //p.667 E.7.4.1.1: rotational velocity is stored as axis*angle
767 //always wrt entity
768 float axis[3];
769 vecnormalize3f(axis,pnode->_angularVelocity.c);
770 vecscale3f(axis,axis,pnode->_angularVelocity.c[3]);
771 vec3f2vector3float(&espdu->deadReckoningParameters.entityAngularVelocity,axis);
772 }
773 //p.675 E.8.2 Use of Other Parameters for standard algorithms 1 through 9
774 switch(pnode->deadReckoning){
775 //fixed rotation
776 case 1:
777 case 2:
778 case 5:
779 case 6:
780 case 9:
781 {
782 float ypr[3];
784 if(pnode->__geoSystem){
785 axisangle2ypr(pnode->rotation.c,ypr); //assume Transform.rotation is wrt TCS/LGS
786 }else{
787 vecset3f(ypr,0.0f,0.0f,0.0f); //we assume we are in local
788 }
789 veccopy3f((float*)&espdu->deadReckoningParameters.otherParameters[3],ypr);
790 }
791 break;
792 //rotating
793 case 3:
794 case 7:
795 case 8:
796 {
797 // p.677 E.8.2.3.2 Issuance of orientation quaternion
798 // a 'squished quaternion'
799 Quaternion qglobal;
800 double quat4d[4];
801 float quat4f[4];
802 unsigned int iquat0;
803 unsigned short iquat16;
805 if(pnode->__geoSystem){
806 //H: W2B = W2L x L2B
807 // World2body = world2local x local2body
808 // where world2local is the gc2tcs (geocentric to topocentric aka local geodetic system) from geospatial
809 // and local2body is the espdu.transform.(translation and rotation) (or its inverse)
810 Quaternion qtcs, qlocal;
811 struct SFVec3d gc, gd, translate;
812 struct SFVec4d rotate;
813 Geosys *gs;
814 gs = GEOSYS(pnode->__geoSystem);
815 user2gc(gs,&pnode->geoCoords,1,&gc);
816 //gc2gd(gs,&gc,1,&gd);
817 //gc2tcs_transform(gs,&gd,&translate,&rotate);
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);
822 }else{
823 vrmlrot_to_quaternion(&qglobal,0.0,1.0,0.0,0.0); //we've already combined DR with global, so additional DR is zero
824 }
825 quat2double(quat4d,&qglobal); //w in last slot of quat4d
826 double2float(&quat4f[1],quat4d,3);
827 quat4f[0] = quat4d[3]; //now w in first slot of quat4f
828 if(quat4f[0] < 0.0f)
829 vecscale4f(quat4f,quat4f,-1.0f);
830
831 iquat0 = (unsigned int)(quat4f[0] * 65536);
832 if(quat4f[0] > 65536) iquat0 = 65535;
833 iquat16 = iquat0;
834 memcpy(&espdu->deadReckoningParameters.otherParameters[1],&iquat16,sizeof(short));
835 memcpy(&espdu->deadReckoningParameters.otherParameters[3],&quat4f[1],3*sizeof(float));
836 }
837 break;
838 default:
839 espdu->deadReckoningParameters.otherParameters[0] = (char)0;
840 break;
841 }
842
843 }else{ //geo
844 //non-geo scene
845 //Apr 22, 2018 we no longer use this, but keeping until benchmark against Brutzman
846 //doesn't necessarily make sense to have no geoSystem or geoCoords = 0,0,0
847 //but some old/existing scenes are like that, so here we handling them
848 //but whether node.translation is meant to be tcs2body or global2body might make a difference?
849 //we don't know because we only have freewrl for testing right now - will wait for brutzman
850 if(0){
851 double local2bodyxyz[3], localxyz[3];
852 float2double(local2bodyxyz,pnode->translation.c,3);
853 tcs2localswizzled(localxyz,local2bodyxyz);
854 vec3d2vector3double(&espdu->entityLocation,localxyz);
855 }else{
856 espdu->entityLocation.x = pnode->translation.c[0];
857 espdu->entityLocation.y = -pnode->translation.c[2]; //??? is this right?
858 espdu->entityLocation.z = pnode->translation.c[1];
859 }
860 //rotation
861 if(0){
862 //theirs:
863 //X PSI
864 //Y THETA
865 //Z PHI
866 //(x, -z, y)
867 //OURS THEIRS THEIRS
868 //x X=x PSI
869 //y Z=y PHI
870 //z -Y=z -THETA
871
872 Quaternion qA;
873 double ypr[3];
874 float *c = pnode->rotation.c;
875 vrmlrot_to_quaternion(&qA,c[0],c[1],c[2],c[3]);
876 quat2euler(ypr,0,&qA);
877 espdu->entityOrientation.psi = ypr[1];
878 espdu->entityOrientation.theta = ypr[2];
879 }
880 if(1){
881 float ypr[3];
882 axisangle2ypr(pnode->rotation.c,ypr);
883 espdu->entityOrientation.psi = -ypr[0]; //gimbal.js shows -yaw
884 espdu->entityOrientation.theta = ypr[1];
885 espdu->entityOrientation.phi = ypr[2];
886 }
887 //dead reckoning > send
888 if(1){
889 //first update linear V,A, angularV
890 //all of which are in Local/TCS for us
891 pnode->_change_count++;
892 if(pnode->_change_count > 1){
893 double dtime;
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);
897 //pnode->_change_count = min(pnode->_change_count,2);
898 if(pnode->_change_count > 2){
899 //a = (v1-v0)/dt
900 vecscale3f(a1,vecdif3f(tmp,v1,pnode->linearVelocity.c),1.0f/dtime);
901 veccopy3f(pnode->linearAcceleration.c,a1);
902 //v1 = v0 - 1/2at**2
903
904 }else{
905 vecset3f(pnode->linearAcceleration.c,0.0,0.0,0.0);
906 }
907 veccopy3f(pnode->linearVelocity.c,v1);
908 {
909 //update angular velocity
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;
917 }
918 }
919 veccopy3f(pnode->_lastp0.c,pnode->translation.c);
920 veccopy4f(pnode->_lastr0.c,pnode->rotation.c);
921 pnode->_lastp0time = TickTime();
922 }
923 espdu->deadReckoningParameters.deadReckoningAlgorithm = pnode->deadReckoning;
924 vec3f2vector3float(&espdu->entityLinearVelocity,pnode->linearVelocity.c);
925 vec3f2vector3float(&espdu->deadReckoningParameters.entityLinearAcceleration,pnode->linearAcceleration.c);
926
927 {
928 //p.667 E.7.4.1.1: rotational velocity is stored as axis*angle
929 //always wrt entity
930 float axis[3];
931 vecnormalize3f(axis,pnode->_angularVelocity.c);
932 vecscale3f(axis,axis,pnode->_angularVelocity.c[3]);
933 vec3f2vector3float(&espdu->deadReckoningParameters.entityAngularVelocity,axis);
934 }
935 //p.675 E.8.2 Use of Other Parameters for standard algorithms 1 through 9
936 switch(pnode->deadReckoning){
937 //fixed rotation
938 case 1:
939 case 2:
940 case 5:
941 case 6:
942 case 9:
943 {
944 float ypr[3];
946 if(pnode->__geoSystem){
947 axisangle2ypr(pnode->rotation.c,ypr); //assume Transform.rotation is wrt TCS/LGS
948 }else{
949 vecset3f(ypr,0.0f,0.0f,0.0f); //we assume we are in local
950 }
951 veccopy3f((float*)&espdu->deadReckoningParameters.otherParameters[3],ypr);
952 }
953 break;
954 //rotating
955 case 3:
956 case 7:
957 case 8:
958 {
959 // p.677 E.8.2.3.2 Issuance of orientation quaternion
960 // a 'squished quaternion'
961 Quaternion qglobal;
962 double quat4d[4];
963 float quat4f[4];
964 unsigned int iquat0;
965 unsigned short iquat16;
967 if(pnode->__geoSystem){
968 //H: W2B = W2L x L2B
969 // World2body = world2local x local2body
970 // where world2local is the gc2tcs (geocentric to topocentric aka local geodetic system) from geospatial
971 // and local2body is the espdu.transform.(translation and rotation) (or its inverse)
972 Quaternion qtcs, qlocal;
973 struct SFVec3d gc, gd, translate;
974 struct SFVec4d rotate;
975 Geosys *gs;
976 gs = GEOSYS(pnode->__geoSystem);
977 user2gc(gs,&pnode->geoCoords,1,&gc);
978 //gc2gd(gs,&gc,1,&gd);
979 //gc2tcs_transform(gs,&gd,&translate,&rotate);
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);
984 }else{
985 vrmlrot_to_quaternion(&qglobal,0.0,1.0,0.0,0.0); //we've already combined DR with global, so additional DR is zero
986 }
987 quat2double(quat4d,&qglobal); //w in last slot of quat4d
988 double2float(&quat4f[1],quat4d,3);
989 quat4f[0] = quat4d[3]; //now w in first slot of quat4f
990 if(quat4f[0] < 0.0f)
991 vecscale4f(quat4f,quat4f,-1.0f);
992
993 iquat0 = (unsigned int)(quat4f[0] * 65536);
994 if(quat4f[0] > 65536) iquat0 = 65535;
995 iquat16 = iquat0;
996 memcpy(&espdu->deadReckoningParameters.otherParameters[1],&iquat16,sizeof(short));
997 memcpy(&espdu->deadReckoningParameters.otherParameters[3],&quat4f[1],3*sizeof(float));
998 }
999 break;
1000 default:
1001 espdu->deadReckoningParameters.otherParameters[0] = (char)0;
1002 break;
1003 }
1004 } //if geo else
1005 //vecprint3fb("trans=",pnode->translation.c,"\n");
1006 pnode->_sent = TRUE;
1007 //articuation parameters
1008 if(pnode->articulationParameterArray.n){
1009 struct ArticulationParameter *ap;
1010 int i, np = pnode->articulationParameterArray.n;
1011 ap = malloc(np * sizeof(struct ArticulationParameter));
1013 //printf("sending %d articulation parameters:\n",np);
1014 for(i=0;i<np;i++){
1015 ap[i].parameterTypeDesignator = 0; //0 is articulated part
1016 ap[i].parameterType = 1029; //1024 - rudder + 5 X
1017 ap[i].parameterValue = pnode->articulationParameterArray.p[i];
1018 ap[i].partAttachedTo = 0;
1019 //printf("%d %f\n",i,pnode->articulationParameterArray.p[i]);
1020 }
1021 espdu->articulationParameters = (void*)ap;
1022 }
1023 node2pdu_entityType(&pnode->entityKind,&espdu->entityType);
1024
1025 //...
1026 //printf("new espdu protocol %d type %d\n",espdu->myEntityInformationFamilyPdu.myPdu.protocolVersion,espdu->myEntityInformationFamilyPdu.myPdu.pduType);
1027 vector_pushBack(struct Pdu*,pdus,(struct Pdu*)espdu);
1028 }
1029
1030 //ephemerals / expendables - no hearbeat requirements?
1031 //FIRE
1032 if(pnode->_pduchange_fire){
1033 struct FirePdu *fpdu;
1034 fpdu = (struct FirePdu *) dis_ctor(type_FirePdu);
1035 fpdu->myWarfareFamilyPdu.firingEntityID.entity = pnode->entityID;
1036 fpdu->myWarfareFamilyPdu.firingEntityID.application = pnode->applicationID;
1037 fpdu->myWarfareFamilyPdu.firingEntityID.site = pnode->siteID;
1038 //fpdu->myWarfareFamilyPdu.myPdu;
1039 //fpdu->myWarfareFamilyPdu.targetEntityID;
1040 printf("FIREPDU ");
1041 if(pnode->fired1 || pnode->fired2){
1042 pnode->firedTime = TickTime();
1043 }
1044 //copy from espdutransform node to pdu
1045 fpdu->burstDescriptor.fuse = pnode->fuse;
1046 struct X3D_EspduTransform * muni = (struct X3D_EspduTransform * )dis_find_registered_node_by_entityid(pnode->munitionEntityID,TRUE,FALSE);
1047 if(muni){
1048 fpdu->burstDescriptor.munition.category = muni->entityCategory; //get munition entity from pnode->munitionEntity, then munitionEntity.cateogory.
1049 fpdu->burstDescriptor.munition.country = muni->entityCountry;
1050 fpdu->burstDescriptor.munition.domain = muni->entityDomain;
1051 fpdu->burstDescriptor.munition.entityKind = muni->entityKind;
1052 fpdu->burstDescriptor.munition.extra = muni->entityExtra;
1053 fpdu->burstDescriptor.munition.specific = muni->entitySpecific;
1054 fpdu->burstDescriptor.munition.subcategory = muni->entitySubCategory;
1055 }
1056 fpdu->burstDescriptor.quantity = pnode->munitionQuantity;
1057 fpdu->burstDescriptor.rate = pnode->firingRate;
1058 fpdu->burstDescriptor.warhead = pnode->warhead;
1059
1060 fpdu->eventID.application = 0;
1061 fpdu->eventID.eventNumber = pnode->eventNumber; //dis_next_event_number(); //increment in sender script node
1062 fpdu->eventID.site = 0; //target if known
1063
1064 fpdu->fireMissionIndex = dis_next_fire_mission_index();
1065 {
1066 double loc[3];
1067 float2double(loc,pnode->munitionStartPoint.c,3);
1068 //am I supposed to convert to wworld from local here?
1069 //or can/should I assume that its local to Weapon Espdu here, and local to target scene's copy of Weapon Espdu?
1070 vec3d2vector3double(&fpdu->locationInWorldCoordinates,loc); //is this current location, or starting location?
1071 }
1072 //unique munition entity if known
1073 fpdu->munitionID.application = pnode->munitionApplicationID;
1074 fpdu->munitionID.entity = pnode->munitionEntityID;
1075 fpdu->munitionID.site = pnode->munitionSiteID;
1076
1077 fpdu->range = pnode->firingRange;
1078 {
1079 float delta[3];
1080 vecdif3f(delta,pnode->munitionEndPoint.c,pnode->munitionStartPoint.c);
1081 //lets say 3 seconds to deliver any munition
1082 vecscale3f(delta,delta,1.0f/3.0f);
1083 vec3f2vector3float(&fpdu->velocity,delta);
1084 }
1085 vector_pushBack(struct Pdu*,pdus,(struct Pdu*)fpdu);
1086 }
1087 //COLLISION
1088 if(pnode->_pduchange_collision){
1089 struct CollisionPdu *cpdu;
1090 cpdu = (struct CollisionPdu *) dis_ctor(type_CollisionPdu);
1091 //copy from espdutransform node to pdu
1092 cpdu->issuingEntityID.application = pnode->applicationID;
1093 cpdu->issuingEntityID.site = pnode->siteID;
1094 cpdu->issuingEntityID.entity = pnode->entityID;
1095
1096 cpdu->collidingEntityID.entity = pnode->eventEntityID;
1097 cpdu->collidingEntityID.application = pnode->eventApplicationID;
1098 cpdu->collidingEntityID.site = pnode->eventSiteID;
1099 cpdu->collisionType = pnode->collisionType;
1100 cpdu->eventID.eventNumber = pnode->eventNumber;
1101
1102 vector_pushBack(struct Pdu*,pdus,(struct Pdu*)cpdu);
1103 }
1104 //DETONATION
1105 if(pnode->_pduchange_detonation){
1106 struct DetonationPdu *dpdu;
1107 dpdu = (struct DetonationPdu *) dis_ctor(type_DetonationPdu);
1108 //copy from espdutransform node to pdu
1109
1110 dpdu->myWarfareFamilyPdu.firingEntityID.entity = pnode->entityID;
1111 dpdu->myWarfareFamilyPdu.firingEntityID.application = pnode->applicationID;
1112 dpdu->myWarfareFamilyPdu.firingEntityID.site = pnode->siteID;
1113 //fpdu->myWarfareFamilyPdu.myPdu;
1114 //fpdu->myWarfareFamilyPdu.targetEntityID;
1115
1116 pnode->detonateTime = TickTime();
1117 dpdu->detonationResult = pnode->detonationResult;
1118 //copy from espdutransform node to pdu
1119 dpdu->burstDescriptor.fuse = pnode->fuse;
1120 struct X3D_EspduTransform * muni = (struct X3D_EspduTransform * )dis_find_registered_node_by_entityid(pnode->munitionEntityID,TRUE,FALSE);
1121 if(muni){
1122 dpdu->burstDescriptor.munition.category = muni->entityCategory; //get munition entity from pnode->munitionEntity, then munitionEntity.cateogory.
1123 dpdu->burstDescriptor.munition.country = muni->entityCountry;
1124 dpdu->burstDescriptor.munition.domain = muni->entityDomain;
1125 dpdu->burstDescriptor.munition.entityKind = muni->entityKind;
1126 dpdu->burstDescriptor.munition.extra = muni->entityExtra;
1127 dpdu->burstDescriptor.munition.specific = muni->entitySpecific;
1128 dpdu->burstDescriptor.munition.subcategory = muni->entitySubCategory;
1129 }
1130 dpdu->burstDescriptor.quantity = pnode->munitionQuantity;
1131 dpdu->burstDescriptor.rate = pnode->firingRate;
1132 dpdu->burstDescriptor.warhead = pnode->warhead;
1133
1134 dpdu->eventID.application = 0;
1135 dpdu->eventID.eventNumber = pnode->eventNumber; //dis_next_event_number(); //increment in sender script node
1136 dpdu->eventID.site = 0; //target if known
1137
1138 //articuation parameters
1139 if(pnode->articulationParameterArray.n){
1140 struct ArticulationParameter *ap;
1141 int i, np = pnode->articulationParameterArray.n;
1142 ap = malloc(np * sizeof(struct ArticulationParameter));
1144 //printf("sending %d articulation parameters:\n",np);
1145 for(i=0;i<np;i++){
1146 ap[i].parameterTypeDesignator = 0; //0 is articulated part
1147 ap[i].parameterType = 1029; //1024 - rudder + 5 X
1148 ap[i].parameterValue = pnode->articulationParameterArray.p[i];
1149 ap[i].partAttachedTo = 0;
1150 //printf("%d %f\n",i,pnode->articulationParameterArray.p[i]);
1151 }
1152 dpdu->articulationParameters = (void*)ap;
1153 }
1154
1155 {
1156 double loc[3];
1157 float2double(loc,pnode->detonationLocation.c,3);
1158 //am I supposed to convert to wworld from local here?
1159 //or can/should I assume that its local to Weapon Espdu here, and local to target scene's copy of Weapon Espdu?
1160 vec3d2vector3double(&dpdu->locationInWorldCoordinates,loc);
1161 vec3f2vector3float(&dpdu->locationInEntityCoordinates,pnode->detonationRelativeLocation.c);
1162 }
1163 //unique munition entity if known
1164 dpdu->munitionID.application = pnode->munitionApplicationID;
1165 dpdu->munitionID.entity = pnode->munitionEntityID;
1166 dpdu->munitionID.site = pnode->munitionSiteID;
1167
1168 {
1169 float delta[3];
1170 vecdif3f(delta,pnode->munitionEndPoint.c,pnode->munitionStartPoint.c);
1171 //lets say .5 seconds to detonate any munition
1172 vecscale3f(delta,delta,1.0f/.5f);
1173 vec3f2vector3float(&dpdu->velocity,delta);
1174 }
1175
1176
1177 vector_pushBack(struct Pdu*,pdus,(struct Pdu*)dpdu);
1178 }
1179
1180 return pdus;
1181
1182}
1183
1184struct Vector * dis_node2pdus_receiver(struct X3D_Node *node, int isHeartbeat){
1185 struct Vector *pdus;
1186 struct X3D_ReceiverPdu * pnode = (struct X3D_ReceiverPdu*)node;
1187 pdus = newVector(struct Pdu *, 6);
1188 // Q. what about network sensor>
1189 // Q. what about _geoCoords?
1190 if(pnode->_pduchange_receiver){
1191 struct ReceiverPdu *rpdu;
1192 rpdu = (struct ReceiverPdu *) dis_ctor(type_ReceiverPdu);
1193//SFInt32 [in,out] radioID 0 [0,65535]
1194//SFFloat [in,out] receivedPower 0.0 [0,?)
1195//SFInt32 [in,out] receiverState 0 [0,65535]
1196//SFInt32 [in,out] transmitterApplicationID 1 [0,65535]
1197//SFInt32 [in,out] transmitterEntityID 0 [0,65535]
1198//SFInt32 [in,out] transmitterRadioID 0 [0,65535]
1199//SFInt32 [in,out] transmitterSiteID 0 [0,65535]
1200 rpdu->myRadioCommunicationsFamilyPdu.radioId = pnode->radioID;
1201 rpdu->receivedPoser = pnode->receivedPower;
1202 rpdu->receiverState = pnode->receiverState;
1203 rpdu->transmitterRadioId = pnode->transmitterRadioID;
1204 rpdu->transmitterEntityId.entity = pnode->transmitterEntityID;
1205 rpdu->transmitterEntityId.site = pnode->transmitterSiteID;
1206 rpdu->transmitterEntityId.application = pnode->transmitterSiteID;
1207 vector_pushBack(struct Pdu*,pdus,(struct Pdu*)rpdu);
1208 }
1209 return pdus;
1210}
1211struct Vector * dis_node2pdus_transmitter(struct X3D_Node *node, int isHeartbeat){
1212 struct Vector *pdus;
1213 struct X3D_TransmitterPdu * pnode = (struct X3D_TransmitterPdu*)node;
1214 pdus = newVector(struct Pdu *, 6);
1215
1216 // Q. what about network sensor>
1217 // Q. what about _geoCoords?
1218 if(pnode->_pduchange_transmitter){
1219 struct TransmitterPdu *tpdu;
1220 tpdu = (struct TransmitterPdu *) dis_ctor(type_TransmitterPdu);
1221
1222 //SFVec3f [in,out] antennaLocation 0 0 0 (-8,8)
1223 //SFInt32 [in,out] antennaPatternLength 0 [0,65535]
1224 //SFInt32 [in,out] antennaPatternType 0 [0,65535]
1225 //SFInt32 [in,out] cryptoKeyID 0 [0,65535]
1226 //SFInt32 [in,out] cryptoSystem 0 [0,65535]
1227 //SFInt32 [in,out] frequency 0
1228 //SFInt32 [in,out] inputSource 0 [0,255]
1229 //SFInt32 [in,out] lengthOfModulationParameters 0 [0,255]
1230 //SFInt32 [in,out] modulationTypeDetail 0 [0,65535]
1231 //SFInt32 [in,out] modulationTypeMajor 0 [0,65535]
1232 //SFInt32 [in,out] modulationTypeSpreadSpectrum 0 [0,65535]
1233 //SFInt32 [in,out] modulationTypeSystem 0 [0,65535]
1234 //SFFloat [in,out] power 0.0 [0,8)
1235 //SFInt32 [in,out] radioEntityTypeCategory 0 [0,255]
1236 //SFInt32 [in,out] radioEntityTypeCountry 0 [0,65535]
1237 //SFInt32 [in,out] radioEntityTypeDomain 0 [0,255]
1238 //SFInt32 [in,out] radioEntityTypeKind 0 [0,255]
1239 //SFInt32 [in,out] radioEntityTypeNomenclature 0 [0,255]
1240 //SFInt32 [in,out] radioEntityTypeNomenclatureVersion 0 [0,65535]
1241 //SFInt32 [in,out] radioID 0 [0,255]
1242 //SFVec3f [in,out] relativeAntennaLocation 0 0 0 (-8,8)
1243 //SFFloat [in,out] transmitFrequencyBandwidth 0.0 (-8,8)
1244 //SFInt32 [in,out] transmitState 0 [0,255]
1245 {
1246 double loc[3];
1247 float2double(loc,pnode->antennaLocation.c,3);
1248 vec3d2vector3double(&tpdu->antennaLocation,loc);
1249 vec3f2vector3float(&tpdu->relativeAntennaLocation,pnode->relativeAntennaLocation.c);
1250 }
1251 tpdu->antennaPatternCount = pnode->antennaPatternLength;
1252 tpdu->antennaPatternType = pnode->antennaPatternType;
1253 tpdu->cryptoKeyId = pnode->cryptoKeyID;
1254 tpdu->cryptoSystem = pnode->cryptoSystem;
1255 tpdu->frequency = pnode->frequency;
1256 tpdu->inputSource = pnode->inputSource;
1257 tpdu->modulationType.detail = pnode->modulationTypeDetail;
1258 tpdu->modulationType.major = pnode->modulationTypeMajor;
1259 tpdu->modulationType.spreadSpectrum = pnode->modulationTypeSpreadSpectrum;
1260 tpdu->modulationType.system = pnode->modulationTypeSystem;
1261 // memcpy(tpdu->modulationParametersList, ????) we have no field for it
1262 tpdu->modulationParameterCount = pnode->lengthOfModulationParameters; //==0 since no field for parameters
1263 tpdu->power = pnode->power;
1264 tpdu->radioEntityType.category = pnode->radioEntityTypeCategory;
1265 tpdu->radioEntityType.country = pnode->radioEntityTypeCountry;
1266 tpdu->radioEntityType.domain = pnode->radioEntityTypeDomain;
1267 tpdu->radioEntityType.entityKind = pnode->radioEntityTypeKind;
1268 tpdu->radioEntityType.nomenclature = pnode->radioEntityTypeNomenclature;
1269 tpdu->radioEntityType.nomenclatureVersion = pnode->radioEntityTypeNomenclatureVersion;
1270 tpdu->myRadioCommunicationsFamilyPdu.radioId = pnode->radioID;
1271 tpdu->transmitFrequencyBandwidth = pnode->transmitFrequencyBandwidth;
1272 tpdu->transmitState = pnode->transmitState;
1273
1274 vector_pushBack(struct Pdu*,pdus,(struct Pdu*)tpdu);
1275 }
1276 return pdus;
1277}
1278#define ONE_INT32_PER_SIGNAL_DATA_BYTE TRUE
1279struct Vector * dis_node2pdus_signal(struct X3D_Node *node, int isHeartbeat){
1280 struct Vector *pdus;
1281 struct X3D_SignalPdu * pnode = (struct X3D_SignalPdu*)node;
1282 pdus = newVector(struct Pdu *, 6);
1283
1284 // Q. what about network sensor>
1285 // Q. what about _geoCoords?
1286 if(pnode->_pduchange_signal){
1287 struct SignalPdu *spdu;
1288 spdu = (struct SignalPdu *) dis_ctor(type_SignalPdu);
1289
1290 //MFInt32 [in,out] data [] [0,255]
1291 //SFInt32 [in,out] dataLength 0 [0,65535]
1292 //SFInt32 [in,out] encodingScheme 0 [0,65535]
1293 //SFInt32 [in,out] radioID 0 [0,65535]
1294 //SFInt32 [in,out] sampleRate 0 [0,65535]
1295 //SFInt32 [in,out] samples 0 [0,65535]
1296 //SFInt32 [in,out] tdlType 0 [0,65535]
1297 spdu->data = realloc(spdu->data,pnode->dataLength*sizeof(unsigned char));
1298 if(ONE_INT32_PER_SIGNAL_DATA_BYTE){
1299 int k;
1300 unsigned char *cdata = (unsigned char *)spdu->data;
1301 for(k=0;k<spdu->dataLength;k++){
1302 cdata[k] = (unsigned char)((unsigned int)pnode->data.p[k] % 256);
1303 }
1304 }else{
1305 memcpy(spdu->data,pnode->data.p,pnode->dataLength);
1306 spdu->dataLength = pnode->dataLength;
1307 }
1308 spdu->encodingScheme = pnode->encodingScheme;
1309 spdu->sampleRate = pnode->sampleRate;
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;
1315 spdu->myRadioCommunicationsFamilyPdu.entityId.application = pnode->applicationID;
1316 vector_pushBack(struct Pdu*,pdus,(struct Pdu*)spdu);
1317 }
1318 return pdus;
1319}
1320
1321// for incoming pdus, we tag them as we handle them
1322// in pdu->padding
1323enum {
1324 TAG_UNCLAIMED = 0,
1325 TAG_SAME_PROGRAM = 1,
1326 TAG_ESPDU = 2,
1327 TAG_ENTITY_MANAGER = 3,
1328 TAG_RECEIVER = 4,
1329 TAG_TRANSMITTER = 5,
1330 TAG_SIGNAL = 6,
1331 TAG_AVATAR = 7,
1332 TAG_SENSOR = 8,
1333};
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){
1336 int i, ihit;
1337 struct Pdu* pdu;
1338 struct X3D_EspduTransform * pnode = (struct X3D_EspduTransform*)node;
1339
1340 ihit = 0;
1341 if(!pdus) return ihit;
1342 for(i=0;i<pdus->n;i++)
1343 {
1344 pdu = vector_get(struct Pdu*,pdus,i);
1345 if(pdu->padding == TAG_UNCLAIMED)
1346 switch(pdu->pduType){
1347 case PDU_ENTITY_STATE:
1348 {
1349 //ENTITYSTATE
1350 struct EntityStatePdu *espdu;
1351 espdu = (struct EntityStatePdu*)pdu;
1352 // 2018.pdf 6.2.80.3 Application Number implied an instance number,
1353 // so should not be same unless loopback testing
1354 //if (espdu->entityID.application == pnode->applicationID
1355 // && espdu->entityID.site == pnode->siteID) {
1356 if (espdu->entityID.application == fwl_get_DISapplication()
1357 && espdu->entityID.site == fwl_get_DISsite()) {
1358 pdu->padding = TAG_SAME_PROGRAM;
1359 break;
1360 }
1361 int avatar = FALSE;
1362 if (espdu->entityType.category == 77) avatar = TRUE;
1363 // break; //its an avatar, handled elsewhere
1364 if(espdu->entityID.entity != pnode->entityID) break;
1365 ihit++;
1366 pdu->padding = avatar ? TAG_AVATAR : TAG_ESPDU;
1367 pnode->_change++; //mark node changed
1368 pnode->timestamp = TickTime();
1369 if(pnode->__geoSystem){
1370 Quaternion qgc2tcs, qtcs2body, qgc2body;
1371 struct SFVec3d gd, gc, translate;
1372 struct SFVec4d rotate;
1373 double localxyz[3], tcsxyz[3], tcs2bodyxyz[3], world2bodyxyz[3];
1374 float xyza[4];
1375 Geosys *gs;
1376 gs = GEOSYS(pnode->__geoSystem);
1377 user2gc(gs,&pnode->geoCoords,1,&gc);
1378 //gc2gd(gs,&gc,1,&gd);
1379 //gc2tcs_transform(gs,&gd,&translate,&rotate);
1380 gc2tcsB_transform(gs,&gc,&translate,&rotate);
1381 //somehow get body/entity into world/gc - rotation and translation
1382 {
1383 //rotation
1384 //assumption (Apr 2018 don't know how Xj3d does it, here's dug9's guess):
1385 // pdu is world2body
1386 // espdutransform.rotation = local2body
1387 // - where local is TCS Topocentric Coord System as described for GeoLocation
1388 // - and world is GC
1389 // local2body = world2local.inverse x world2body
1390 // for freewrl world2local is gc2tcs
1391 Quaternion qtcs2gc, q;
1392 float ypr[3], xyza[4];
1393 ypr[0] = -espdu->entityOrientation.psi;
1394 ypr[1] = espdu->entityOrientation.theta;
1395 ypr[2] = espdu->entityOrientation.phi;
1396 ypr2axisangle(ypr,xyza);
1397 xyza[3] = -xyza[3];
1398 //vecprint4fb("recv xyza",xyza,"\n");
1399
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);
1406 }
1407 {
1408 //translation - dug9 debate: could do it one of 2 ways
1409 enum transmethod {
1410 TRANS_ZERO = 1,
1411 TRANS_LOCATION_MINUS_GEOCOORD = 2
1412 };
1413 //static int transmethod = TRANS_LOCATION_MINUS_GEOCOORD;
1414 static int transmethod = TRANS_ZERO;
1415
1416 vector3double2vec3d(world2bodyxyz,&espdu->entityLocation);
1417 if(transmethod == TRANS_LOCATION_MINUS_GEOCOORD){
1418 //METHOD 1: translation = Location - geoCoords
1419 struct SFVec3d world, tcs;
1420 veccopyd(world.c,world2bodyxyz);
1421 //gc2tcs(gs,&gd,&world,1,&tcs);
1422 gc2tcsB(gs,&gc,&world,1,&tcs);
1423 double2float(pnode->translation.c,tcs.c,3);
1424 }else{
1425 //TRANS_ZERO
1426 //METHOD 2: geoCoords = Location; translation = 000
1427 // x smoothing doesn't work if done in translation / tcs space
1428 struct SFVec3d world, tcs2, tcs1;
1429 double deltatcs[3];
1430 float deltap[3];
1431 static int want_smoothing = 1;
1432 if(want_smoothing){
1433 //gc2tcs(gs,&gd,&gc,1,&tcs1);
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);
1441 }
1442 gc2user(gs,&world,1,&pnode->geoCoords);
1443 //node->_change++;
1444 vecset3f(pnode->translation.c,0.0f,0.0f,0.0f);
1445
1446 }
1447 }
1448 // recv geo dead reckoning
1449 pnode->deadReckoning = espdu->deadReckoningParameters.deadReckoningAlgorithm;
1450 {
1451 float V[3], A[3];
1452 // in entity or world, depending on drmethod
1453 vector3float2vec3f(A,&espdu->deadReckoningParameters.entityLinearAcceleration);
1454 vector3float2vec3f(V,&espdu->entityLinearVelocity);
1455
1456 //convert linear/angular V,A from world or Entity, to local
1457 //http://movesinstitute.org/~mcgredo/MV3500/hla/1278.1-200X%20Draft%2016%20rev%2018.pdf
1458 //p.333, p.329
1459 if(pnode->deadReckoning < 6){
1460 //convert world to TCS/Local
1461 //vtcs = gc2tcs x Vworld
1462 Quaternion q;
1463 vrmlrot4d_to_quaternion(&q,rotate.c);
1464 quaternion_rotation3f(V,&q,V);
1465 quaternion_rotation3f(A,&q,A);
1466 } else {
1467 if(0){
1468 //convert entity to TCS/Local
1469 //Vtcs = body2tcs x Vbody
1470 Quaternion q;
1471 vrmlrot4f_to_quaternion(&q,pnode->rotation.c);
1472 quaternion_rotation3f(V,&q,V);
1473 quaternion_rotation3f(A,&q,A);
1474 }else{
1475 //keep entity in entity
1476 }
1477 }
1478
1479 veccopy3f(pnode->linearAcceleration.c,A);
1480 veccopy3f(pnode->linearVelocity.c,V);
1481
1482 }
1483 {
1484 //p.667 E.7.4.1.1: rotational velocity is stored as axis*angle, in entity space
1485 float axis[3], angle;
1486 vector3float2vec3f(axis,&espdu->deadReckoningParameters.entityAngularVelocity);
1487 angle = veclength3f(axis);
1488 vecnormalize3f(pnode->_angularVelocity.c,axis);
1489 pnode->_angularVelocity.c[3] = angle;
1490 }
1491 if (disverbose() && pdu->padding == TAG_AVATAR) {
1492 printf("Avatar S%2d A%2d T %lf\n", espdu->entityID.site, espdu->entityID.application,TickTime());
1493 }
1494 }else{
1495 //non-geosystem scene. Apr 22, 2018 we aren't using this now
1496 // -- everything goes through geosystem code above
1497 // -- but keeping this until we benchmark against Brutzman
1498 //translation - assumes companion scenes will have same parent transform stack
1499 //(x, -z, y).
1500
1501 pnode->translation.c[0] = espdu->entityLocation.x;
1502 pnode->translation.c[1] = espdu->entityLocation.z;
1503 pnode->translation.c[2] = -espdu->entityLocation.y;
1504 //rotation
1505 if(0){
1506 Quaternion qA;
1507 float ypr[3];
1508 double r[4];
1509 float *c = pnode->rotation.c;
1510 ypr[0] = espdu->entityOrientation.phi;
1511 ypr[1] = espdu->entityOrientation.psi;
1512 ypr[2] = espdu->entityOrientation.theta;
1513 euler2quat(&qA,ypr[0],ypr[1],ypr[2]);
1514 //quaternion_normalize(&qA);
1515 //vrmlrot_to_quaternion(&qA,c[0],c[1],c[2],c[3]);
1516 quaternion_to_vrmlrot(&qA,&r[0],&r[1],&r[2],&r[3]);
1517 c[0] = (float)r[0];
1518 c[1] = (float)r[1];
1519 c[2] = (float)r[2];
1520 c[3] = (float)r[3];
1521 }
1522 if(1){
1523 float ypr[3];
1524 ypr[0] = -espdu->entityOrientation.psi; //gimbal.js shows -yaw
1525 ypr[1] = espdu->entityOrientation.theta;
1526 ypr[2] = espdu->entityOrientation.phi;
1527 ypr2axisangle(ypr,pnode->rotation.c);
1528 }
1529 // dead reckoning
1530 pnode->deadReckoning = espdu->deadReckoningParameters.deadReckoningAlgorithm;
1531 vector3float2vec3f(pnode->linearAcceleration.c,&espdu->deadReckoningParameters.entityLinearAcceleration);
1532 vector3float2vec3f(pnode->linearVelocity.c,&espdu->entityLinearVelocity);
1533 {
1534 //p.667 E.7.4.1.1: rotational velocity is stored as axis*angle
1535 float axis[3], angle;
1536 vector3float2vec3f(axis,&espdu->deadReckoningParameters.entityAngularVelocity);
1537 angle = veclength3f(axis);
1538 vecnormalize3f(pnode->_angularVelocity.c,axis);
1539 pnode->_angularVelocity.c[3] = angle;
1540 }
1541
1542 }
1543 //articuation parameters
1544 pnode->articulationParameterArray.n = espdu->numberOfArticulationParameters;
1545 //printf("recv art count %d\n",espdu->numberOfArticulationParameters);
1546 if(pnode->articulationParameterArray.n){
1547 struct ArticulationParameter *ap;
1548 float *pp;
1549 int i, np = pnode->articulationParameterArray.n;
1550 ap = espdu->articulationParameters;
1551 pp = malloc(np * sizeof(float));
1552 //printf("received %d articulation parameters:\n",np);
1553 for(i=0;i<np;i++){
1554 //ap[i].parameterTypeDesignator = 0; //0 is articulated part
1555 //ap[i].parameterType = 1029; //1024 - rudder + 5 X
1556 pp[i] = (float)ap[i].parameterValue;
1557 //printf("%d %f\n",i,pp[i]);
1558 //ap[i].partAttachedTo = 0;
1559 switch(i){
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;
1568 default:
1569 break;
1570 }
1571 }
1572 if(pnode->articulationParameterArray.p) free(pnode->articulationParameterArray.p);
1573 pnode->articulationParameterArray.p = pp;
1574 //done in generic mark_changed_fields //MARK_EVENT(X3D_NODE(pnode),offsetof(struct X3D_EspduTransform,articulationParameterArray));
1575 }
1576 pdu2node_entityType(&espdu->entityType,&pnode->entityKind);
1577 pnode->_pduchange_es = TRUE;
1578 if(espdu->entityAppearance | 1 << 20){
1579 //http://movesinstitute.org/~mcgredo/MV3500/hla/1278.1-200X%20Draft%2016%20rev%2018.pdf
1580 //p.50 no dead reckoning if isFrozen bit is set, bit 21 of entityAppearance
1581 //(why can't they just leave dead reckoning parameters 0, and run through formula? H: specs written in 1990s for 80386 processors)
1582 //pnode->_isFrozen = TRUE; //pduchange_es = FALSE;
1583 }
1584
1585 //...
1586 }
1587 break;
1588 case PDU_FIRE:
1589 {
1590 //FIRE
1591 struct FirePdu *fpdu;
1592 fpdu = (struct FirePdu*)pdu;
1593 if(fpdu->myWarfareFamilyPdu.firingEntityID.application != pnode->applicationID) break;
1594 if(fpdu->myWarfareFamilyPdu.firingEntityID.site != pnode->siteID) break;
1595 if(fpdu->myWarfareFamilyPdu.firingEntityID.entity != pnode->entityID) break;
1596
1597 //fpdu->myWarfareFamilyPdu.myPdu;
1598 //fpdu->myWarfareFamilyPdu.targetEntityID;
1599
1600
1601 ihit++;
1602 pnode->_change++; //mark node changed
1603 pnode->timestamp = TickTime();
1604
1605 //if(pnode->fired1 || pnode->fired2){
1606 pnode->firedTime = TickTime();
1607 //}
1608 pnode->fired1 = TRUE;
1609 //copy from espdutransform node to pdu
1610 pnode->fuse = fpdu->burstDescriptor.fuse;
1611 //pnode->munitionEntityID = fpdu->burstDescriptor.munition.
1612 //the following doesn't work in target scene
1613 struct X3D_EspduTransform * muni;
1614 // kind, domain, country, category, subcategory, specific, extra
1615 muni = (struct X3D_EspduTransform * )dis_find_or_create_espdu_by_category(
1622 fpdu->burstDescriptor.munition.extra
1623 );
1624 if(!muni) printf("no muni\n");
1625 else {
1626 printf("got muni\n");
1627 pnode->munitionEntityID = muni->entityID;
1628 pnode->munitionSiteID = muni->siteID;
1629 pnode->munitionApplicationID = muni->applicationID;
1630 }
1631 pnode->munitionQuantity = fpdu->burstDescriptor.quantity;
1632 pnode->firingRate = fpdu->burstDescriptor.rate;
1633 pnode->warhead = fpdu->burstDescriptor.warhead;
1634
1635 //fpdu->eventID.application = 0;
1636 pnode->eventNumber = fpdu->eventID.eventNumber; //dis_next_event_number(); //increment in sender script node
1637 //fpdu->eventID.site = 0; //target if known
1638
1639 pnode->fireMissionIndex = fpdu->fireMissionIndex;
1640 {
1641 double loc[3];
1642 //am I supposed to convert to wworld from local here?
1643 //or can/should I assume that its local to Weapon Espdu here, and local to target scene's copy of Weapon Espdu?
1644 vector3double2vec3d(loc,&fpdu->locationInWorldCoordinates); //is this current location, or starting location?
1645 double2float(pnode->munitionStartPoint.c,loc,3);
1646 }
1647 //unique munition entity if known
1648 pnode->munitionApplicationID = fpdu->munitionID.application;
1649 pnode->munitionEntityID = fpdu->munitionID.entity;
1650 pnode->munitionSiteID = fpdu->munitionID.site;
1651
1652 //fpdu->myWarfareFamilyPdu.firingEntityID;
1653 //fpdu->myWarfareFamilyPdu.myPdu;
1654 //fpdu->myWarfareFamilyPdu.targetEntityID;
1655 pnode->firingRange = fpdu->range;
1656 {
1657 float delta[3];
1658 vector3float2vec3f(delta,&fpdu->velocity);
1659 //lets say 3 seconds to deliver any munition
1660 vecscale3f(delta,delta,3.0f/1.0f);
1661 vecadd3f(pnode->munitionEndPoint.c,pnode->munitionStartPoint.c,delta);
1662 }
1663 pnode->_pduchange_fire = TRUE;
1664 }
1665 break;
1666 case PDU_COLLISION:
1667 {
1668 //COLLISION
1669 struct CollisionPdu *cpdu;
1670 cpdu = (struct CollisionPdu *)pdu;
1671
1672 if(cpdu->issuingEntityID.application != pnode->applicationID) break;
1673 if(cpdu->issuingEntityID.site != pnode->siteID) break;
1674 if(cpdu->issuingEntityID.entity != pnode->entityID) break;
1675 pnode->eventEntityID = cpdu->collidingEntityID.entity;
1676 pnode->eventApplicationID = cpdu->collidingEntityID.application;
1677 pnode->eventSiteID = cpdu->collidingEntityID.site;
1678 pnode->collisionType = cpdu->collisionType;
1679 if(pnode->collisionType) pnode->isCollided = TRUE;
1680 else pnode->isCollided = FALSE;
1681 //cpdu->eventID;
1682 //cpdu->location;
1683 //cpdu->mass;
1684 //cpdu->myEntityInformationFamilyPdu.myPdu.exerciseID;
1685 //cpdu->velocity;
1686 pnode->_pduchange_collision = TRUE;
1687 }
1688 break;
1689 case PDU_DETONATION:
1690 {
1691 //DETONATION
1692 struct DetonationPdu *dpdu;
1693 dpdu = (struct DetonationPdu*)pdu;
1694 if(dpdu->myWarfareFamilyPdu.firingEntityID.application != pnode->applicationID) break;
1695 if(dpdu->myWarfareFamilyPdu.firingEntityID.site != pnode->siteID) break;
1696 if(dpdu->myWarfareFamilyPdu.firingEntityID.entity != pnode->entityID) break;
1697
1698 ihit++;
1699 pnode->_change++; //mark node changed
1700 pnode->timestamp = TickTime();
1701
1702 pnode->detonateTime = TickTime();
1703 pnode->fuse = dpdu->burstDescriptor.fuse;
1704 //pnode->munitionEntityID = fpdu->burstDescriptor.munition.
1705 //the following doesn't work in target scene
1706 struct X3D_EspduTransform * muni;
1707 // kind, domain, country, category, subcategory, specific, extra
1708 muni = (struct X3D_EspduTransform * )dis_find_or_create_espdu_by_category(
1715 dpdu->burstDescriptor.munition.extra
1716 );
1717 if(!muni) printf("no muni\n");
1718 else {
1719 printf("got muni\n");
1720 pnode->munitionEntityID = muni->entityID;
1721 pnode->munitionSiteID = muni->siteID;
1722 pnode->munitionApplicationID = muni->applicationID;
1723 }
1724 pnode->munitionQuantity = dpdu->burstDescriptor.quantity;
1725 pnode->firingRate = dpdu->burstDescriptor.rate;
1726 pnode->warhead = dpdu->burstDescriptor.warhead;
1727
1728 //fpdu->eventID.application = 0;
1729 pnode->eventNumber = dpdu->eventID.eventNumber; //dis_next_event_number(); //increment in sender script node
1730 //unique munition entity if known
1731 pnode->munitionApplicationID = dpdu->munitionID.application;
1732 pnode->munitionEntityID = dpdu->munitionID.entity;
1733 pnode->munitionSiteID = dpdu->munitionID.site;
1734 {
1735 double loc[3];
1736 vector3double2vec3d(loc,&dpdu->locationInWorldCoordinates);
1737 double2float(pnode->detonationLocation.c,loc,3);
1738 vector3float2vec3f(pnode->detonationRelativeLocation.c,&dpdu->locationInEntityCoordinates);
1739 }
1740 pnode->detonationResult = dpdu->detonationResult;
1741
1742 {
1743 float delta[3];
1744 vector3float2vec3f(delta,&dpdu->velocity);
1745 //lets say .5 seconds to explode a munition
1746 vecscale3f(delta,delta,.5f/1.0f);
1747 veccopy3f(pnode->munitionStartPoint.c,pnode->detonationRelativeLocation.c);
1748 vecadd3f(pnode->munitionEndPoint.c,pnode->munitionStartPoint.c,delta);
1749 }
1750 //articuation parameters
1751 pnode->articulationParameterArray.n = dpdu->numberOfArticulationParameters;
1752 //printf("recv art count %d\n",espdu->numberOfArticulationParameters);
1753 if(pnode->articulationParameterArray.n){
1754 struct ArticulationParameter *ap;
1755 float *pp;
1756 int i, np = pnode->articulationParameterArray.n;
1757 ap = dpdu->articulationParameters;
1758 pp = malloc(np * sizeof(float));
1759 //printf("received %d articulation parameters:\n",np);
1760 for(i=0;i<np;i++){
1761 //ap[i].parameterTypeDesignator = 0; //0 is articulated part
1762 //ap[i].parameterType = 1029; //1024 - rudder + 5 X
1763 pp[i] = (float)ap[i].parameterValue;
1764 //printf("%d %f\n",i,pp[i]);
1765 //ap[i].partAttachedTo = 0;
1766 switch(i){
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;
1775 default:
1776 break;
1777 }
1778 }
1779 if(pnode->articulationParameterArray.p) free(pnode->articulationParameterArray.p);
1780 pnode->articulationParameterArray.p = pp;
1781 //done in generic mark_changed_fields //MARK_EVENT(X3D_NODE(pnode),offsetof(struct X3D_EspduTransform,articulationParameterArray));
1782 }
1783 pnode->_pduchange_detonation = TRUE;
1784
1785 }
1786 break;
1787 default:
1788 break;
1789 }
1790 }
1791 return ihit;
1792}
1793
1794int dis_pdus2node_receiver(struct X3D_Node *node, struct Vector *pdus){
1795 int i, ihit;
1796 struct Pdu* pdu;
1797 struct X3D_ReceiverPdu * pnode = (struct X3D_ReceiverPdu*)node;
1798
1799 ihit = 0;
1800 if(!pdus) return ihit;
1801 for(i=0;i<pdus->n;i++)
1802 {
1803 pdu = vector_get(struct Pdu*,pdus,i);
1804 if(pdu->padding == TAG_UNCLAIMED)
1805 switch(pdu->pduType){
1806 case PDU_RECEIVER:
1807 {
1808 struct ReceiverPdu *rpdu;
1809 rpdu = (struct ReceiverPdu*)pdu;
1810
1811 if(pnode->radioID != rpdu->myRadioCommunicationsFamilyPdu.radioId) break;
1812 ihit++;
1813 pdu->padding = TAG_RECEIVER;
1814 pnode->_change++; //mark node changed
1815 pnode->timestamp = TickTime();
1816
1817 pnode->receivedPower = rpdu->receivedPoser; //spelling Poser / Power
1818 pnode->receiverState = rpdu->receiverState;
1819 pnode->transmitterRadioID = rpdu->transmitterRadioId;
1820 pnode->transmitterEntityID = rpdu->transmitterEntityId.entity;
1821 pnode->transmitterSiteID = rpdu->transmitterEntityId.site;
1822 pnode->transmitterSiteID = rpdu->transmitterEntityId.application;
1823 pnode->_pduchange_receiver = TRUE;
1824 }
1825 break;
1826 default:
1827 break;
1828 }
1829 }
1830 return ihit;
1831}
1832int dis_pdus2node_transmitter(struct X3D_Node *node, struct Vector *pdus){
1833 int i, ihit;
1834 struct Pdu* pdu;
1835 struct X3D_TransmitterPdu * pnode = (struct X3D_TransmitterPdu*)node;
1836
1837 ihit = 0;
1838 if(!pdus) return ihit;
1839 for(i=0;i<pdus->n;i++)
1840 {
1841 pdu = vector_get(struct Pdu*,pdus,i);
1842 if(pdu->padding == TAG_UNCLAIMED)
1843 switch(pdu->pduType){
1844 case PDU_TRANSMITTER:
1845 {
1846 struct TransmitterPdu *tpdu;
1847 tpdu = (struct TransmitterPdu*)pdu;
1848
1849 if(pnode->radioID != tpdu->myRadioCommunicationsFamilyPdu.radioId) break;
1850 if(tpdu->myRadioCommunicationsFamilyPdu.entityId.entity != pnode->entityID) break;
1851 //if(tpdu->myRadioCommunicationsFamilyPdu.entityId.site != pnode->siteID) break;
1852 //site.application should not be the same unless loopback testing
1853 //if(tpdu->myRadioCommunicationsFamilyPdu.entityId.application == pnode->applicationID) break;
1854
1855 ihit++;
1856 pdu->padding = TAG_TRANSMITTER;
1857 pnode->_change++; //mark node changed
1858 pnode->timestamp = TickTime();
1859 {
1860 double loc[3];
1861 vector3double2vec3d(loc,&tpdu->antennaLocation);
1862 double2float(pnode->antennaLocation.c,loc,3);
1863 vector3float2vec3f(pnode->relativeAntennaLocation.c,&tpdu->relativeAntennaLocation);
1864 }
1865 pnode->antennaPatternLength = tpdu->antennaPatternCount;
1866 pnode->antennaPatternType = tpdu->antennaPatternType;
1867 pnode->cryptoKeyID = tpdu->cryptoKeyId;
1868 pnode->cryptoSystem = tpdu->cryptoSystem;
1869 pnode->frequency = tpdu->frequency;
1870 pnode->inputSource = tpdu->inputSource;
1871 pnode->modulationTypeDetail = tpdu->modulationType.detail;
1872 pnode->modulationTypeMajor = tpdu->modulationType.major;
1873 pnode->modulationTypeSpreadSpectrum = tpdu->modulationType.spreadSpectrum;
1874 pnode->modulationTypeSystem = tpdu->modulationType.system;
1875 // memcpy(tpdu->modulationParametersList, ????) we have no field for it
1876 pnode->lengthOfModulationParameters = tpdu->modulationParameterCount; //==0 since no field for parameters
1877 pnode->power = tpdu->power;
1878 pnode->radioEntityTypeCategory = tpdu->radioEntityType.category;
1879 pnode->radioEntityTypeCountry = tpdu->radioEntityType.country;
1880 pnode->radioEntityTypeDomain = tpdu->radioEntityType.domain;
1881 pnode->radioEntityTypeKind = tpdu->radioEntityType.entityKind;
1882 pnode->radioEntityTypeNomenclature = tpdu->radioEntityType.nomenclature;
1883 pnode->radioEntityTypeNomenclatureVersion = tpdu->radioEntityType.nomenclatureVersion;
1884 pnode->radioID = tpdu->myRadioCommunicationsFamilyPdu.radioId;
1885 pnode->transmitFrequencyBandwidth = tpdu->transmitFrequencyBandwidth;
1886 pnode->transmitState = tpdu->transmitState;
1887 pnode->_pduchange_transmitter = TRUE;
1888
1889 }
1890 break;
1891 default:
1892 break;
1893 }
1894 }
1895 return ihit;
1896}
1897// #define ONE_INT32_PER_SIGNAL_DATA_BYTE TRUE
1898int dis_pdus2node_signal(struct X3D_Node *node, struct Vector *pdus){
1899 int i, ihit;
1900 struct Pdu* pdu;
1901 struct X3D_SignalPdu * pnode = (struct X3D_SignalPdu*)node;
1902
1903 ihit = 0;
1904 if(!pdus) return ihit;
1905 for(i=0;i<pdus->n;i++)
1906 {
1907 pdu = vector_get(struct Pdu*,pdus,i);
1908 if(pdu->padding == TAG_UNCLAIMED)
1909 switch(pdu->pduType){
1910 case PDU_SIGNAL:
1911 {
1912 struct SignalPdu *spdu;
1913 spdu = (struct SignalPdu*)pdu;
1914
1915 if(pnode->radioID != spdu->myRadioCommunicationsFamilyPdu.radioId) break;
1916 if(spdu->myRadioCommunicationsFamilyPdu.entityId.entity != pnode->entityID) break;
1917 //site won't necessarily be the same
1918 //if(spdu->myRadioCommunicationsFamilyPdu.entityId.site != pnode->siteID) break;
1919 //site.application should not be the same unless loopback testing
1920 //if(spdu->myRadioCommunicationsFamilyPdu.entityId.application == pnode->applicationID) break;
1921
1922 ihit++;
1923 pdu->padding = TAG_SIGNAL;
1924 pnode->_change++; //mark node changed
1925 pnode->timestamp = TickTime();
1926 if(ONE_INT32_PER_SIGNAL_DATA_BYTE){
1927 int k;
1928 unsigned char *cdata = (unsigned char *)spdu->data;
1929 pnode->data.p = realloc(pnode->data.p,spdu->dataLength*sizeof(int));
1930 for(k=0;k<spdu->dataLength;k++){
1931 pnode->data.p[k] = (int)(cdata[k]);
1932 }
1933 }else{
1934 memcpy(pnode->data.p,spdu->data,pnode->dataLength);
1935 pnode->data.n = (spdu->dataLength + 4) / 4;
1936 }
1937 pnode->dataLength = spdu->dataLength;
1938 pnode->encodingScheme = spdu->encodingScheme;
1939 pnode->sampleRate = spdu->sampleRate;
1940 pnode->samples = spdu->samples;
1941 pnode->tdlType = spdu->tdlType;
1942 pnode->_pduchange_signal = TRUE;
1943 }
1944 break;
1945 default:
1946 break;
1947 }
1948 }
1949 return ihit;
1950}
1951
1952
1953
1954// Simulation Management PDUs relate to the DISEntityManager node
1955// http://movesinstitute.org/~mcgredo/MV3500/hla/1278.1-200X%20Draft%2016%20rev%2018.pdf x dead link see tests/28_DIS for .pdf copy
1956//5.6 Simulation management p.85
1957//6.2.82 Simulation Management PDU Header record p.311
1958//- its an abstract type
1959//- the pdutype burried in the standard header part is the implied ACTION. ie create, or remove.
1960//Table 114 p.313:
1961//PDU Reference Originating ID Receiving ID
1962//Create Entity 5.6.5.2 Simulation ID Entity ID or Special Create Entity Identifier
1963//Remove Entity 5.6.5.3 Simulation ID Entity ID
1964//Table 12 p.87
1965//Table 3 p.31 - IDs, including specials: All Simulations, no particular node: ALL_SITES 65535, ALL_APPLIC = 65535, RefID 0
1966//5.6.5.2 Create Entity PDU p.88
1967
1968
1969struct Vector * dis_node2pdus_sm(struct X3D_Node *node, int isHeartbeat){
1970
1971 struct Vector *pdus;
1972 struct X3D_DISEntityManager * pnode = (struct X3D_DISEntityManager*)node;
1973 pdus = newVector(struct Pdu *, 6);
1974
1975 //ENTITYSTATE
1976 //if(pnode->_pduchange_es_articulation || pnode->_pduchange_es_deadreckoning || pnode->_pduchange_es_info || pnode->_pduchange_es_force){
1977 printf("em pduchange create %d remove %d heartbeat %d ticktime %lf\n",pnode->_pduchange_create, pnode->_pduchange_remove, isHeartbeat,TickTime());
1978 if(isHeartbeat){
1979 //lets say someone joins the exercise late.
1980 //how do they get synched up?
1981 }
1982 {
1983 struct SimulationManagementPdu *simanpdu;
1984 }
1985 //CREATE
1986 if(pnode->_pduchange_create){
1987 struct CreateEntityPdu *crpdu;
1988 crpdu = (struct CreateEntityPdu *) dis_ctor(type_CreateEntityPdu);
1989 //copy from espdutransform node to pdu
1990 crpdu->mySimulationManagementFamilyPdu.originatingEntityID.entity = pnode->entityID;
1991 //crpdu->mySimulationManagementFamilyPdu.receivingEntityID = ALL_SITES; ???
1992 //crpdu->requestID = ??
1994 vector_pushBack(struct Pdu*,pdus,(struct Pdu*)crpdu);
1995 }
1996 //REMOVE
1997 if(pnode->_pduchange_remove){
1998 struct RemoveEntityPdu *rmpdu;
1999 rmpdu = (struct RemoveEntityPdu *) dis_ctor(type_RemoveEntityPdu);
2000 //copy from espdutransform node to pdu
2001 vector_pushBack(struct Pdu*,pdus,(struct Pdu*)rmpdu);
2002 }
2003 return pdus;
2004
2005}
2006int dis_pdus2node_sm(struct X3D_Node *node, struct Vector *pdus){
2007 int i, ihit;
2008 struct Pdu* pdu;
2009 struct X3D_DISEntityManager * pnode = (struct X3D_DISEntityManager*)node;
2010
2011 ihit = 0;
2012 if(!pdus) return ihit;
2013 for(i=0;i<pdus->n;i++)
2014 {
2015 pdu = vector_get(struct Pdu*,pdus,i);
2016 switch(pdu->pduType){
2017 case PDU_CREATE_ENTITY:
2018 {
2019 //CREATE
2020 struct CreateEntityPdu *crpdu;
2021 //crpdu->mySimulationManagementFamilyPdu.myPdu.
2022 printf("hi from pdu2node create_entity\n");
2023 pdu->padding = TAG_ENTITY_MANAGER;
2024 ihit++;
2025 pnode->_pduchange_create = TRUE;
2026 }
2027 break;
2028 case PDU_REMOVE_ENTITY:
2029 {
2030 //REMOVE
2031 struct RemoveEntityPdu *rmpdu;
2032 printf("hi from pdu2node remove_entity\n");
2033 pdu->padding = TAG_ENTITY_MANAGER;
2034 ihit++;
2035 pnode->_pduchange_remove = TRUE;
2036 }
2037 break;
2038
2039 default:
2040 break;
2041 }
2042 }
2043 return ihit;
2044}
2045void dis_set_node_lasttime(struct X3D_Node *node, double lasttime){
2046 //4 nodes have the same field order for common fields, can be cast to Espdu
2047 switch(node->_nodeType){
2048 case NODE_ReceiverPdu:
2049 case NODE_TransmitterPdu:
2050 case NODE_SignalPdu:
2051 case NODE_EspduTransform:
2052 case NODE_DISEntityManager:
2053 {
2054 struct X3D_EspduTransform *pnode = (struct X3D_EspduTransform*)node;
2055 pnode->_lasttime = lasttime;
2056 }
2057 break;
2058 break;
2059 }
2060}
2061
2062int dis_pdus2newnode(struct dis_socket *dsock, struct X3D_DISEntityManager *pnode, struct Vector * pdus){
2063 int ihit = 0;
2064 if(pnode){
2065 int i;
2066 // http://www.web3d.org/documents/specifications/19775-1/V3.3/Part01/components/dis.html#DISEntityManager
2067 // https://github.com/open-dis/DISTutorial/blob/master/EntityDiscovery.md
2068 // entity discovery happening here
2069 struct Pdu* pdu;
2070 for(i=0;i<pdus->n;i++) {
2071 pdu = vector_get(struct Pdu*,pdus,i);
2072 if(pdu->padding == TAG_UNCLAIMED)
2073 switch(pdu->pduType){
2074 case PDU_ENTITY_STATE:
2075 case PDU_RECEIVER:
2076 case PDU_TRANSMITTER:
2077 case PDU_SIGNAL:
2078 case PDU_ENTITY_STATE_UPDATE:
2079 {
2080 int j, already_done;
2081 int entityID, siteID, applicationID;
2082 struct EntityStatePdu *espdu;
2083 struct X3D_EspduTransform* et;
2084 espdu = (struct EntityStatePdu*)pdu;
2085
2086 //don't send to yourself
2087 //if (pnode->applicationID == espdu->entityID.application &&
2088 // pnode->siteID == espdu->entityID.site) {
2089 if (espdu->entityID.application == fwl_get_DISapplication() &&
2090 espdu->entityID.site == fwl_get_DISsite()) {
2091 pdu->padding = TAG_SAME_PROGRAM;
2092 //ihit++;
2093 break;
2094 }
2095 //skip if we already got this entity and are just awaiting creation
2096 already_done = FALSE;
2097 //printf("addEntities.n = %d\n", pnode->addEntities.n);
2098 //printf("espdu entity %d app %d site %d\n", espdu->entityID.entity, espdu->entityID.application, espdu->entityID.site);
2099 for(j=0;j<pnode->addEntities.n;j++){
2100 struct X3D_Node *candi = (struct X3D_Node*)pnode->addEntities.p[j];
2101 if(candi->_nodeType == NODE_DISEntityTypeMapping){
2102 struct X3D_DISEntityTypeMapping *et = (struct X3D_DISEntityTypeMapping *)candi;
2103 //already_done = FALSE;
2104 }else if(candi->_nodeType == NODE_EspduTransform) {
2105 // || candi->_nodeType == NODE_ReceiverPdu
2106 // || candi->_nodeType == NODE_TransmitterPdu || candi->_nodeType == NODE_SignalPdu){
2107 //else if radio etc
2108 struct X3D_EspduTransform *et = (struct X3D_EspduTransform *)candi;
2109 if(et->entityID == espdu->entityID.entity &&
2110 et->applicationID == espdu->entityID.application &&
2111 et->siteID == espdu->entityID.site) already_done = TRUE;
2112 //printf("addEntities[%d] entity %d app %d site %d already %d\n",
2113 // j,et->entityID, et->applicationID, et->siteID, already_done);
2114 if(already_done) break;
2115 }
2116
2117 }
2118 if (already_done) {
2119 //printf("already done\n");
2120 break;
2121 }
2122 //printf("not already done\n");
2123 pdu->padding = TAG_ENTITY_MANAGER;
2124 ihit++;
2125 //we'll use an EspduTransform struct just as a temp struct, not to register.
2126 // -for the purpose of communicating with whatever can create a local copy
2127 // of a discovered entity.
2128 // right now, that's our EntityManager node.
2129 et = createNewX3DNode0(NODE_EspduTransform); //the 0 creator which does not register the node
2130 int nodetype = 0;
2131 switch(pdu->pduType){
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;
2137 default: break;
2138 }
2139 //copy world coordinates as approx GC, in case < earths radius / 2 (earths core) test later, we use GC instead of default GD,WE
2140 vector3double2vec3d(et->geoCoords.c,&espdu->entityLocation);
2141
2142 et->_nodeType = nodetype;
2143 et->applicationID = espdu->entityID.application;
2144 et->siteID = espdu->entityID.site;
2145 et->entityID = espdu->entityID.entity;
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;
2150 et->entityCategory = espdu->entityType.category;
2151 et->entityCountry = espdu->entityType.country;
2152 et->entityDomain = espdu->entityType.domain;
2153 et->entityKind = espdu->entityType.entityKind;
2154 et->entityExtra = espdu->entityType.extra;
2155 et->entitySpecific = espdu->entityType.specific;
2156 et->entitySubCategory = espdu->entityType.subcategory;
2157
2158 {
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++;
2163 // >> do I need pnode->_pduchange_create = TRUE;
2164 }
2165 // ?? do I need MARK_EVENT(X3D_NODE(pnode),offsetof (struct X3D_DISEntityManager, addEntities));
2166 //will get mapped and instanced as geom during entityManager scenegraph visit and compile
2167 // >> pnode->_change ++;
2168 //ihit = 1;
2169 }
2170 break;
2171 default:
2172 break;
2173 }
2174 }
2175 }
2176 return ihit;
2177}
2178int dis_entity_retire(struct X3D_DISEntityManager *pnode, struct X3D_Node *node){
2179 //we only retire the entities that were created by 'entity_discovery'
2180 int iret = 0;
2181 if(node->_nodeType == NODE_EspduTransform){
2182 if(pnode && pnode->_nodeType == NODE_DISEntityManager){
2183 int i;
2184 static int ADD = 1, REMOVE = 2;
2185 iret = -1;
2186 for(i=0;i<pnode->entities.n;i++){
2187 if(pnode->entities.p[i] == node){
2188 //yes - created by entity discovery
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__);
2191
2192 iret = 1;
2193 break;
2194 }
2195 }
2196 }
2197 }
2198 return iret;
2199}
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);
2205 break;
2206 case NODE_DISEntityManager:
2207 pdus = dis_node2pdus_sm(node,isHeartbeat);
2208 break;
2209 case NODE_ReceiverPdu:
2210 pdus = dis_node2pdus_receiver(node,isHeartbeat);
2211 break;
2212 case NODE_TransmitterPdu:
2213 pdus = dis_node2pdus_transmitter(node,isHeartbeat);
2214 break;
2215 case NODE_SignalPdu:
2216 pdus = dis_node2pdus_signal(node,isHeartbeat);
2217 break;
2218 break;
2219 }
2220 return pdus;
2221}
2222static struct Vector *sockets_send = NULL;
2223static struct Vector *sockets_recv = NULL;
2224
2225struct X3D_Node * dis_find_registered_node_by_entityid(int entityid, int sendlist, int recvlist){
2226 int i,j;
2227 struct X3D_Node *node, *pnode = NULL;
2228 if(sendlist)
2229 for(i=0;i<sockets_send->n;i++){
2230 struct dis_socket *dsock = vector_get_ptr(struct dis_socket,sockets_send,i);
2231 if(dsock->registered){
2232 for(j=0;j<dsock->registered->n;j++){
2233 int ihit;
2234 struct X3D_Node *node = vector_get(struct X3D_Node*,dsock->registered,j);
2235 if(node->_nodeType == NODE_EspduTransform){
2236 struct X3D_EspduTransform *espdu = (struct X3D_EspduTransform *)node;
2237 if(espdu->entityID == entityid){
2238 pnode = node;
2239 break;
2240 }
2241 }
2242 }
2243 }
2244 }
2245 if(recvlist)
2246 for(i=0;i<sockets_recv->n;i++){
2247 struct dis_socket *dsock = vector_get_ptr(struct dis_socket,sockets_recv,i);
2248 if(dsock->registered){
2249 for(j=0;j<dsock->registered->n;j++){
2250 int ihit;
2251 struct X3D_Node *node = vector_get(struct X3D_Node*,dsock->registered,j);
2252 if(node->_nodeType == NODE_EspduTransform){
2253 struct X3D_EspduTransform *espdu = (struct X3D_EspduTransform *)node;
2254 if(espdu->entityID == entityid){
2255 pnode = node;
2256 break;
2257 }
2258 }
2259 }
2260 }
2261 }
2262 return pnode;
2263}
2264
2265
2266 // kind, domain, country, category, subcategory, specific, extra
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;
2269 struct X3D_EspduTransform *best = NULL;
2270 ibest = -1;
2271 iscore = 0;
2272 //check EM already-instanced list
2273 for(i=0;i<sockets_recv->n;i++){
2274 struct dis_socket *dsock = vector_get_ptr(struct dis_socket,sockets_recv,i);
2275 if(dsock->registered){
2276 for(j=0;j<dsock->registered->n;j++){
2277 int ihit;
2278 struct X3D_Node *node = vector_get(struct X3D_Node*,dsock->registered,j);
2279 if(node->_nodeType == NODE_DISEntityManager){
2280 struct X3D_DISEntityManager *em = (struct X3D_DISEntityManager *)node;
2281 if(em->entities.n){
2282 for(k=0;k<em->entities.n;k++){
2283 struct X3D_EspduTransform *bnode = (struct X3D_EspduTransform *)em->entities.p[k];
2284 if(bnode->_nodeType == NODE_EspduTransform){
2285 int jscore = 0;
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){
2294 iscore = jscore;
2295 ibest = i;
2296 best = bnode;
2297 }
2298
2299 }
2300 }
2301 }
2302 }
2303 }
2304 }
2305 }
2306 return (struct X3D_Node*)best;
2307}
2308
2309
2310unsigned char buf2[32768];
2311
2312void dis_get_node_lasttime(struct X3D_Node *node, double *lasttime, double *readInterval, double *writeInterval){
2313 //4 nodes have the same field order for common fields, can be cast to Espdu
2314 switch(node->_nodeType){
2315 case NODE_ReceiverPdu:
2316 case NODE_TransmitterPdu:
2317 case NODE_SignalPdu:
2318 case NODE_EspduTransform:
2319 case NODE_DISEntityManager:
2320 {
2321 struct X3D_EspduTransform *pnode = (struct X3D_EspduTransform*)node;
2322 *lasttime = pnode->_lasttime;
2323 *writeInterval = pnode->writeInterval;
2324 *readInterval = pnode->readInterval;
2325 }
2326 break;
2327 default:
2328 break;
2329 }
2330}
2331int node_only_transform_changed(struct X3D_Node *node){
2332 int changed, onlytransform = FALSE;
2333 changed = 0;
2334 if( node->_nodeType == NODE_EspduTransform)
2335 {
2336 struct X3D_EspduTransform *pnode = (struct X3D_EspduTransform *)node;
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;
2341 }
2342 onlytransform = changed == 1;
2343 return onlytransform;
2344}
2345
2346
2347int node_pdus_changed_by_scene(struct X3D_Node *node){
2348 int changed = FALSE;
2349 switch(node->_nodeType){
2350 case NODE_EspduTransform:
2351 {
2352 struct X3D_EspduTransform *pnode = (struct X3D_EspduTransform *)node;
2353 changed = pnode->_pduchange_es;
2354 changed |= pnode->_pduchange_collision;
2355 changed |= pnode->_pduchange_fire;
2356 changed |= pnode->_pduchange_detonation;
2357 }
2358 break;
2359 case NODE_DISEntityManager:
2360 {
2361 struct X3D_DISEntityManager *pnode = (struct X3D_DISEntityManager *)node;
2362 changed = pnode->_pduchange_create;
2363 changed |= pnode->_pduchange_remove;
2364 }
2365 break;
2366 case NODE_TransmitterPdu:
2367 {
2368 struct X3D_TransmitterPdu *pnode = (struct X3D_TransmitterPdu *)node;
2369 changed = pnode->_pduchange_transmitter;
2370 }
2371 break;
2372 case NODE_SignalPdu:
2373 {
2374 struct X3D_SignalPdu *pnode = (struct X3D_SignalPdu *)node;
2375 changed = pnode->_pduchange_signal;
2376 }
2377 break;
2378 case NODE_ReceiverPdu:
2379 {
2380 struct X3D_ReceiverPdu *pnode = (struct X3D_ReceiverPdu *)node;
2381 changed = pnode->_pduchange_receiver;
2382 }
2383 break;
2384 default:
2385 break;
2386 }
2387 return changed;
2388}
2389void reset_node_pduchanged(struct X3D_Node *node){
2390 switch(node->_nodeType){
2391 case NODE_EspduTransform:
2392 {
2393 struct X3D_EspduTransform *pnode = (struct X3D_EspduTransform *)node;
2394 pnode->_pduchange_es = FALSE;
2395 pnode->_pduchange_collision = FALSE;
2396 pnode->_pduchange_fire = FALSE;
2397 pnode->_pduchange_detonation = FALSE;
2398 }
2399 break;
2400 case NODE_DISEntityManager:
2401 {
2402 struct X3D_DISEntityManager *pnode = (struct X3D_DISEntityManager *)node;
2403 pnode->_pduchange_em_info = FALSE;
2404 pnode->_pduchange_create = FALSE;
2405 pnode->_pduchange_remove = FALSE;
2406 }
2407 break;
2408 case NODE_TransmitterPdu:
2409 {
2410 struct X3D_TransmitterPdu *pnode = (struct X3D_TransmitterPdu *)node;
2411 pnode->_pduchange_transmitter = FALSE;
2412 }
2413 break;
2414 case NODE_SignalPdu:
2415 {
2416 struct X3D_SignalPdu *pnode = (struct X3D_SignalPdu *)node;
2417 pnode->_pduchange_signal = FALSE;
2418 }
2419 break;
2420 case NODE_ReceiverPdu:
2421 {
2422 struct X3D_ReceiverPdu *pnode = (struct X3D_ReceiverPdu *)node;
2423 pnode->_pduchange_receiver = FALSE;
2424 }
2425 break;
2426 default:
2427 break;
2428 }
2429}
2430//in socketutils.c:
2431void socket_open(struct dis_socket *dsock);
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);
2436
2437// https://stackoverflow.com/questions/2351087/what-is-the-best-32bit-hash-function-for-short-strings-tag-names
2438// hash: compute hash value of string
2439#define MULTIPLIER 37
2440unsigned int hash37(const char* str)
2441{
2442 unsigned int h;
2443 unsigned char* p;
2444
2445 h = 0;
2446 for (p = (unsigned char*)str; *p != '\0'; p++)
2447 h = MULTIPLIER * h + *p;
2448 return h; // or, h % ARRAY_SIZE; in our case we want it spread over long int 4B so no/rare collisions
2449}
2450const char* getNodeName(struct X3D_Node* node);
2451
2452void dis_recv_sensor(int sensorIndex, int ev, int butStatus2, int status, float* posn3, float* norm3);
2453struct dis_sensor {
2454 int fromNode, dataNode;
2455 int ev, butStatus2;
2456 int status, padding;
2457 float posn3[3], norm3[3];
2458};
2459static struct Vector* sensor_send_queue = NULL; //reset .n to 0 after pdu2buf
2460
2461struct Vector* dis_sensors2pdus() {
2462// 2023 multiplayer experiment: sensor event sharing
2463 //converts queued sensor events into a CommentPdu for sending
2464 struct Vector* pdus = NULL;
2465 if (sensor_send_queue && vectorSize(sensor_send_queue)) {
2466 int nevents = vectorSize(sensor_send_queue); //garbage collect after send
2467 pdus = newVector(struct Pdu*, 1);
2468 struct CommentPdu* cpdu;
2469 cpdu = (struct CommentPdu*)dis_ctor(type_CommentPdu); //garbage collect after send
2470 vector_pushBack(struct Pdu*, pdus, (struct Pdu*)cpdu);
2471 //entity
2472 cpdu->mySimulationManagementFamilyPdu.originatingEntityID.entity = 33; //can be a code for sensors
2473 cpdu->mySimulationManagementFamilyPdu.originatingEntityID.application = fwl_get_DISapplication();
2474 cpdu->mySimulationManagementFamilyPdu.originatingEntityID.site = fwl_get_DISsite();
2475 struct VariableDatum* vd;
2476 vd = malloc(nevents * sizeof(struct VariableDatum)); //free this after pdu2buff
2477 for (int i = 0; i < nevents; i++) {
2478 struct dis_sensor* ds = vector_get_ptr(struct dis_sensor, sensor_send_queue, i);
2479 vd[i].variableDatumID = i;
2480 vd[i].variableDatumLength = 6;
2481 vd[i].variableDatums = (void*)ds; //don't free this
2482 }
2483 cpdu->variableDatums = vd;
2484 cpdu->numberOfVariableDatumRecords = nevents;
2485 //printf("send+");
2486 }
2487 return pdus;
2488}
2489void clear_sensor_queue() {
2490 if(sensor_send_queue) sensor_send_queue->n = 0;
2491}
2492void dis_send_sensor(struct X3D_Node* fromNode, struct X3D_Node* dataNode, int ev, int butStatus2,
2493 int status, float* posn3, float* norm3) {
2494 //receives sensor event information from mainloop sendSensorEvents on event
2495 // and queues for sensor2pdu
2496 // called from Mainloop.c SendSensorEvents about line 6901
2497 if (allow_DIS) {
2498 //enqueue for sending on next dis_send_loop
2499 if (!sensor_send_queue) sensor_send_queue = newStack(struct dis_sensor);
2500 struct dis_sensor ds;
2501 ds.ev = ev;
2502 ds.butStatus2 = butStatus2;
2503 ds.status = status;
2504 veccopy3f(ds.posn3, posn3);
2505 veccopy3f(ds.norm3, norm3);
2506 //node addresses may be different in different app instances
2507 //so we rely on DEF name
2508 //but DEF name can be a medium long string, not good for pdu transmission
2509 //so we convert to 4 byte int with a hash function
2510 int OK = TRUE;
2511 const char* def = getNodeName(fromNode);
2512 if (!def) {
2513 printf("No DEF name for from nodetype %s", stringNodeType(fromNode->_nodeType));
2514 OK = FALSE;
2515 }else
2516 ds.fromNode = hash37(def == NULL ? "" : def);
2517 //printf("send_fromnode def %s has %d ", def, ds.fromNode);
2518 def = getNodeName(dataNode);
2519 if (!def) {
2520 printf("No DEF name for data nodetype %s", stringNodeType(fromNode->_nodeType));
2521 OK = FALSE;
2522 }else
2523 ds.dataNode = hash37(def);
2524 //printf("send_datanode def %s has %d \n", def, ds.dataNode);
2525 if(OK)
2526 stack_push(struct dis_sensor, sensor_send_queue, ds);
2527 }
2528}
2529
2530//struct Vector* sensors = NULL; //2023 multiplayer
2531//void dis_registerSensor(struct X3D_Node* sensor) {
2532// //2023 multiplayer
2533// //call this from somewhere we handle sensor events
2534// if (!sensors) sensors = newVector(struct X3D_Node*, 10);
2535// for (int i = 0; i < sensors->n; i++)
2536// if (sensor == vector_get(struct X3D_Node*, sensors, i)) return; //already registered
2537// vector_pushBack(struct X3D_Node*, sensors, sensor);
2538//}
2539int getSensorCount();
2540void getSensor(int k, struct X3D_Node** fromnode, struct X3D_Node** datanode);
2541
2542int dis_pdus2sensors(struct Vector* pdus) {
2543 //2023 multiplayer
2544 //one sensor update per pdu, or as many as we like?
2545 //we need DEF or ID that's consistent across application instances
2546 //- how about a hash, good for going one way, can't go back
2547 //- so will need a list of registered sensors to compare hash(DEF) with .entity
2548 int i, ihit;
2549 struct Pdu* pdu;
2550
2551 ihit = 0;
2552 if (!pdus || !pdus->n) return ihit;
2553 for (i = 0; i < pdus->n; i++)
2554 {
2555 pdu = vector_get(struct Pdu*, pdus, i);
2556 if (pdu->padding == TAG_UNCLAIMED)
2557 switch (pdu->pduType) {
2558 case PDU_COMMENT:
2559 {
2560 struct CommentPdu* cpdu;
2561 cpdu = (struct CommentPdu*)pdu;
2562 //don't loopback
2563 if (cpdu->mySimulationManagementFamilyPdu.originatingEntityID.application == fwl_get_DISapplication()
2564 && cpdu->mySimulationManagementFamilyPdu.originatingEntityID.site == fwl_get_DISsite()) {
2565 pdu->padding = TAG_SAME_PROGRAM;
2566 //if (disverbose()) printf("SNDR ");
2567 ihit++;
2568 break;
2569 }
2570 if (disverbose()) {
2571 printf("%s ", "COM");
2572 printf("A%2d ", cpdu->mySimulationManagementFamilyPdu.originatingEntityID.application);
2573 printf("S%2d ", cpdu->mySimulationManagementFamilyPdu.originatingEntityID.site);
2574 }
2575
2576 //find matching sensor
2577 struct VariableDatum* vr = cpdu->variableDatums;
2578 if (disverbose()) printf("ND%2d ", cpdu->numberOfVariableDatumRecords);
2579 //printf("vd count %d\n", cpdu->numberOfVariableDatumRecords);
2580 for (int j = 0; j < cpdu->numberOfVariableDatumRecords; j++) {
2581 struct dis_sensor* ds = (struct dis_sensor*)vr[j].variableDatums;
2582 //printf("ds-fromnode %d ds-datanode %d\n", ds->fromNode, ds->dataNode);
2583
2584 int nsensor = getSensorCount();
2585 //printf("nsensor %d\n", nsensor);
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);
2590 //rather than sending and receiving null terminted DEF strings, we'll use 32 bit int hash values
2591 const char *deff, *defd;
2592 deff = getNodeName(fromnode);
2593 int fromNode, dataNode, OK;
2594 OK = TRUE;
2595 if (!deff) OK = FALSE;
2596 else fromNode = hash37(deff);
2597 //printf("recv fromnode def %s hash %d ", def, fromNode);
2598 defd = getNodeName(datanode);
2599 if (!defd) OK = FALSE;
2600 else dataNode = hash37(defd);
2601
2602 //printf("recv datanode def %s hash %d\n", def, dataNode);
2603 int match = OK && ds->fromNode == fromNode && ds->dataNode == dataNode;
2604 if (match) {
2605 dis_recv_sensor(k, ds->ev, ds->butStatus2, ds->status, ds->posn3, ds->norm3);
2606 //printf("recvmatch+");
2607 if(disverbose()) printf("F %s D %s B%d S%d ", deff, defd,ds->butStatus2,ds->status);
2608 break;
2609 }
2610 }
2611 }
2612 pdu->padding = TAG_SENSOR;
2613 ihit++;
2614 //printf("recv+");
2615 if (disverbose()) printf("\n");
2616 break;
2617 }
2618 default:
2619 break;
2620 }
2621 }
2622 return ihit;
2623}
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() {
2628 // 2023 multiplayer experiment: sensor event sharing
2629 struct Vector* pdus = NULL;
2630 if (1) {
2631 //assuming DIS update loop is called from scene root level,
2632 //then modelview should be (at least last frame's) view matrix
2633 //viewmatrix vs vp.position/.orientation: viewmatrix is viewpoint agnostic and in world coords.
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);
2639
2640 transformAFFINEd(pointd, pointd, matinv);
2641 //vecscaled(pointd, pointd, -1.0);
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) {
2652 return pdus;
2653 }
2654 last_time = this_time;
2655
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);
2660 struct EntityStatePdu* espdu;
2661 espdu = (struct EntityStatePdu*)dis_ctor(type_EntityStatePdu);
2662 static int icount = 0;
2663 if(0) printf("Pdu->type = %d i %d\n", ((struct Pdu*)(espdu))->pduType,icount);
2664 icount++;
2665 //entity
2666 espdu->entityType.category = 77; //SPECIAL CATEGORY 77 FOR AVATARS
2667 espdu->entityID.entity = fwl_get_DISapplication(); // application 1:1 avatar, entity = f(application)
2668 espdu->entityID.application = fwl_get_DISapplication();
2669 espdu->entityType.specific = fwl_get_DISapplication();
2670 espdu->entityID.site = fwl_get_DISsite();
2671 if(0) printf("kind %d domain %d country %d category %d sub %d specific %d extra %d\n ",
2674 espdu->entityType.specific, espdu->entityType.extra);
2675
2676 espdu->entityLocation.x = pointd[0];
2677 espdu->entityLocation.y = pointd[1];
2678 espdu->entityLocation.z = pointd[2];
2679 double2float(xyzaf, xyza, 4);
2680 xyzaf[3] = -xyzaf[3];
2681 axisangle2ypr(xyzaf, ypr);
2682 espdu->entityOrientation.psi = -ypr[0]; //gimbal.js shows -yaw
2683 espdu->entityOrientation.theta = ypr[1];
2684 espdu->entityOrientation.phi = ypr[2];
2685 //printf("send xyz %lf %lf %lf ypr %f %f %f\n", pointd[0], pointd[1], pointd[2], ypr[0], ypr[1], ypr[2]);
2686 stack_push(struct Pdu*, pdus, (struct Pdu*)espdu);
2687 }
2688
2689 return pdus;
2690}
2691int write_rtp(unsigned char *buf, struct X3D_Node *node);
2692void dis_sendloop(){
2693 double thistime;
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++){
2698 struct dis_socket *dsock = vector_get_ptr(struct dis_socket,sockets_send,i);
2699 nbytes = 0;
2700 struct Vector* pdus;
2701
2702 if(dsock->registered){
2703 for(j=0;j<dsock->registered->n;j++){
2704 //options:
2705 //a. each node maintains its own pdus every frame on update/compile, and are merely sent here
2706 //b. on send in here, a function is called to pdu-ize a node before marshaling it
2707 //c. like a and b: each node has its own list of pdus for mem, and are updated in here just before send
2708 double lasttime, dtime, readInterval, writeInterval, isHeartbeat;
2709 struct X3D_Node *node = vector_get(struct X3D_Node*,dsock->registered,j);
2710 //printf("registered node type %s\n",stringNodeType(node->_nodeType));
2711 dis_get_node_lasttime(node,&lasttime,&readInterval,&writeInterval);
2712 if(writeInterval == 0.0) continue; //sentinal value 0 means don't write
2713 // finer granularity send decision: a)heartbeat, b)on-change except DR, c) DR dead-reckoning-threshold exceded
2714 // Q. where's our c)dead-reckoning-threshold?
2715 dtime = thistime - lasttime;
2716 isHeartbeat = dtime > writeInterval ? TRUE: FALSE; //a)heartbeat: skip for a while more
2717 if(!isHeartbeat && !node_pdus_changed_by_scene(node)) continue; //b)on-change: no pdus changed since last send, DIS ettiquette says don't send if no change
2718 //if(0) //moved to node _compile/_prep/_child
2719 //if(!isHeartbeat && node_only_transform_changed(node)){
2720 // if(transform_within_DeadReckoningTolerance(node,dtime)) continue;
2721 //}
2722 printf(".\n");
2723 lasttime = thistime;
2724 dsock->lasttime = thistime; //last time something was sent, not needed
2725 if(j==0) {
2726 nb = write_rtp(&buf2[nbytes],node);
2727 nbytes += nb;
2728 }
2729
2730 //option b.
2731 pdus = dis_node2pdus(node,isHeartbeat);
2732 if(isHeartbeat)
2733 dis_set_node_lasttime(node,lasttime);
2734
2735 if(pdus && pdus->n) {
2736 struct Pdu* pdu = vector_get(struct Pdu*,pdus,0);
2737 //printf("in dis_sendloop pdu protocol %d pdutype %d\n",pdu->protocolVersion,pdu->pduType);
2738 }
2739 nb = dis_write_stream(&buf2[nbytes],pdus);
2740 if(0){
2741 //debug
2742 printf("sendloop >>>>\n");
2743 print_stream(&buf2[nbytes], nb);
2744 printf("<<<< sendloop\n");
2745 }
2746 nbytes += nb;
2747 reset_node_pduchanged(node);
2748 }
2749 }
2750 pdus = dis_sensors2pdus();
2751 nb = dis_write_stream(&buf2[nbytes], pdus);
2752 clear_sensor_queue();
2753 nbytes += nb;
2754 pdus = dis_avatar2pdus();
2755 nb = dis_write_stream(&buf2[nbytes], pdus);
2756 nbytes += nb;
2757 if (nbytes) socksendto(dsock, (const char *)buf2, nbytes);
2758 // Q. where is the garbage collection for pdua vector?
2759 // I think the ctor/dtor for pdus works on the pdus themselves.
2760 // But not the pdu vector list I pass around
2761 }
2762}
2763/* RTP Real-time Transport Protocol
2764 optional header that can be on incoming, or put on outgoing
2765 https://en.wikipedia.org/wiki/Real-time_Transport_Protocol
2766 https://calhoun.nps.edu/bitstream/handle/10945/9147/virtualrealitytr00afon.pdf
2767 appendix F, G show DIS header settings and source code for DIS X3D
2768 Version = 2 (default)
2769 M marker = 0
2770 CC csrc_count = 0
2771 P padding = 0
2772 X extension_bit = 0 (no extension header used)
2773 PT payloadType = 111
2774*/
2775struct rtp_header {
2776 unsigned char toprow[2];
2777 unsigned short sequence;
2778 unsigned int timestamp;
2779 unsigned int ssrc;
2780};
2781unsigned char * rtp_strip_header(unsigned char *buf, int *heard){
2782 unsigned char *carat = buf;
2783 //DIS protocoVersion is 6 (1998) or 7 (2009/2012)
2784 //so if the first byte is bigger than that it might be RTP
2785 *heard = FALSE;
2786 if(carat[0] > 127) {
2787 //its an RTP header - strip
2788 struct rtp_header rtph;
2789 int cc, i, x;
2790 *heard = TRUE;
2791 memcpy(&rtph.toprow,carat,2);
2792 carat += 2;
2793 memcpy(&rtph.sequence,carat,2);
2794 carat += 2;
2795 memcpy(&rtph.timestamp,carat,4);
2796 carat += 4;
2797 memcpy(&rtph.ssrc,carat,4);
2798 carat += 4;
2799 cc = rtph.toprow[0] << 4 >> 4;
2800 x = rtph.toprow[0] & 1 << 4;
2801 for(i=0;i<cc;i++){
2802 //skip scrc identifiers
2803 carat += 4;
2804 }
2805 if(x){
2806 //not implemented: extension header skipping
2807 }
2808 }
2809 //in the future, we could do something fancy with the sequence number, like skip stale packets.
2810 return carat;
2811}
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;
2816 PayloadType = 111;
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);
2822 rtph.ssrc = 0;
2823 memcpy(carat,&rtph.toprow,2);
2824 carat += 2;
2825 memcpy(carat,&rtph.sequence,2);
2826 carat += 2;
2827 memcpy(carat,&rtph.timestamp,4);
2828 carat += 4;
2829 memcpy(carat,&rtph.ssrc,4);
2830 carat += 4;
2831 return carat;
2832}
2833int write_rtp(unsigned char *buf, struct X3D_Node *node){
2834 int nb = 0;
2835 int rtue = FALSE;
2836 switch(node->_nodeType){
2837 case NODE_EspduTransform:
2838 rtue = ((struct X3D_EspduTransform *)node)->rtpHeaderExpected;
2839 break;
2840 case NODE_DISEntityManager:
2841 rtue = ((struct X3D_DISEntityManager *)node)->rtpHeaderExpected;
2842 break;
2843 case NODE_ReceiverPdu:
2844 rtue = ((struct X3D_ReceiverPdu *)node)->rtpHeaderExpected;
2845 break;
2846 case NODE_TransmitterPdu:
2847 rtue = ((struct X3D_TransmitterPdu *)node)->rtpHeaderExpected;
2848 break;
2849 case NODE_SignalPdu:
2850 rtue = ((struct X3D_SignalPdu *)node)->rtpHeaderExpected;
2851 break;
2852 default:
2853 break;
2854 }
2855 if(rtue) {
2856 unsigned char *carat;
2857 unsigned int hours, timestamp;
2858 TickTime2DISTime(TickTime(),1,&hours,&timestamp);
2859 carat = rtp_add_header(buf,timestamp);
2860 nb = carat - buf;
2861 }
2862 return nb;
2863}
2864void set_rtp_heard(struct X3D_Node *node){
2865 //would be nice if we had mulitple inheritance techniques for extracting common interfaces
2866 // dis = interface(node,type_DIS)
2867 // dis->isRtpHeaderHeard = TRUE;
2868 switch(node->_nodeType){
2869 case NODE_EspduTransform:
2870 ((struct X3D_EspduTransform *)node)->isRtpHeaderHeard = TRUE;
2871 break;
2872 case NODE_DISEntityManager:
2873 ((struct X3D_DISEntityManager *)node)->isRtpHeaderHeard = TRUE;
2874 break;
2875 case NODE_ReceiverPdu:
2876 ((struct X3D_ReceiverPdu *)node)->isRtpHeaderHeard = TRUE;
2877 break;
2878 case NODE_TransmitterPdu:
2879 ((struct X3D_TransmitterPdu *)node)->isRtpHeaderHeard = TRUE;
2880 break;
2881 case NODE_SignalPdu:
2882 ((struct X3D_SignalPdu *)node)->isRtpHeaderHeard = TRUE;
2883 break;
2884 default:
2885 break;
2886 }
2887}
2888void dis_set_isActive(struct X3D_Node*node, int ival){
2889 switch(node->_nodeType){
2890 case NODE_EspduTransform:
2891 {
2892 struct X3D_EspduTransform* pnode = (struct X3D_EspduTransform*)node;
2893 if(pnode->isActive != ival){
2894 pnode->isActive = ival;
2895 MARK_EVENT(node,offsetof(struct X3D_EspduTransform,isActive));
2896 }
2897 }
2898 break;
2899 case NODE_DISEntityManager:
2900 {
2901 struct X3D_DISEntityManager* pnode = (struct X3D_DISEntityManager*)node;
2902 if(pnode->isActive != ival){
2903 pnode->isActive = ival;
2904 MARK_EVENT(node,offsetof(struct X3D_DISEntityManager,isActive));
2905 }
2906 }
2907 break;
2908 case NODE_ReceiverPdu:
2909 {
2910 struct X3D_ReceiverPdu* pnode = (struct X3D_ReceiverPdu*)node;
2911 if(pnode->isActive != ival){
2912 pnode->isActive = ival;
2913 MARK_EVENT(node,offsetof(struct X3D_ReceiverPdu,isActive));
2914 }
2915 }
2916 break;
2917 case NODE_TransmitterPdu:
2918 {
2919 struct X3D_TransmitterPdu* pnode = (struct X3D_TransmitterPdu*)node;
2920 if(pnode->isActive != ival){
2921 pnode->isActive = ival;
2922 MARK_EVENT(node,offsetof(struct X3D_TransmitterPdu,isActive));
2923 }
2924 }
2925 break;
2926 case NODE_SignalPdu:
2927 {
2928 struct X3D_SignalPdu* pnode = (struct X3D_SignalPdu*)node;
2929 if(pnode->isActive != ival){
2930 pnode->isActive = ival;
2931 MARK_EVENT(node,offsetof(struct X3D_SignalPdu,isActive));
2932 }
2933 }
2934 break;
2935 default:
2936 break;
2937 }
2938}
2939
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;
2947 default: break;
2948 }
2949 switch(node->_nodeType){
2950 case NODE_EspduTransform:
2951 {
2952 struct X3D_EspduTransform* pnode = (struct X3D_EspduTransform*)node;
2953 if(pnode->isStandAlone != isStandAlone){
2954 pnode->isStandAlone = isStandAlone;
2955 MARK_EVENT(node,offsetof(struct X3D_EspduTransform,isStandAlone));
2956 }
2957 if(pnode->isNetworkReader != isNetworkReader){
2958 pnode->isNetworkReader = isNetworkReader;
2959 MARK_EVENT(node,offsetof(struct X3D_EspduTransform,isNetworkReader));
2960 }
2961 if(pnode->isNetworkWriter != isNetworkWriter){
2962 pnode->isNetworkWriter = isNetworkWriter;
2963 MARK_EVENT(node,offsetof(struct X3D_EspduTransform,isNetworkWriter));
2964 }
2965 }
2966 break;
2967 case NODE_DISEntityManager:
2968 {
2969 struct X3D_DISEntityManager* pnode = (struct X3D_DISEntityManager*)node;
2970 if(pnode->isStandAlone != isStandAlone){
2971 pnode->isStandAlone = isStandAlone;
2972 MARK_EVENT(node,offsetof(struct X3D_DISEntityManager,isStandAlone));
2973 }
2974 if(pnode->isNetworkReader != isNetworkReader){
2975 pnode->isNetworkReader = isNetworkReader;
2976 MARK_EVENT(node,offsetof(struct X3D_DISEntityManager,isNetworkReader));
2977 }
2978 if(pnode->isNetworkWriter != isNetworkWriter){
2979 pnode->isNetworkWriter = isNetworkWriter;
2980 MARK_EVENT(node,offsetof(struct X3D_DISEntityManager,isNetworkWriter));
2981 }
2982 }
2983 break;
2984 case NODE_ReceiverPdu:
2985 {
2986 struct X3D_ReceiverPdu* pnode = (struct X3D_ReceiverPdu*)node;
2987 if(pnode->isStandAlone != isStandAlone){
2988 pnode->isStandAlone = isStandAlone;
2989 MARK_EVENT(node,offsetof(struct X3D_EspduTransform,isStandAlone));
2990 }
2991 if(pnode->isNetworkReader != isNetworkReader){
2992 pnode->isNetworkReader = isNetworkReader;
2993 MARK_EVENT(node,offsetof(struct X3D_EspduTransform,isNetworkReader));
2994 }
2995 if(pnode->isNetworkWriter != isNetworkWriter){
2996 pnode->isNetworkWriter = isNetworkWriter;
2997 MARK_EVENT(node,offsetof(struct X3D_EspduTransform,isNetworkWriter));
2998 }
2999 }
3000 break;
3001 case NODE_TransmitterPdu:
3002 {
3003 struct X3D_TransmitterPdu* pnode = (struct X3D_TransmitterPdu*)node;
3004 if(pnode->isStandAlone != isStandAlone){
3005 pnode->isStandAlone = isStandAlone;
3006 MARK_EVENT(node,offsetof(struct X3D_EspduTransform,isStandAlone));
3007 }
3008 if(pnode->isNetworkReader != isNetworkReader){
3009 pnode->isNetworkReader = isNetworkReader;
3010 MARK_EVENT(node,offsetof(struct X3D_EspduTransform,isNetworkReader));
3011 }
3012 if(pnode->isNetworkWriter != isNetworkWriter){
3013 pnode->isNetworkWriter = isNetworkWriter;
3014 MARK_EVENT(node,offsetof(struct X3D_EspduTransform,isNetworkWriter));
3015 }
3016 }
3017 break;
3018 case NODE_SignalPdu:
3019 {
3020 struct X3D_SignalPdu* pnode = (struct X3D_SignalPdu*)node;
3021 if(pnode->isStandAlone != isStandAlone){
3022 pnode->isStandAlone = isStandAlone;
3023 MARK_EVENT(node,offsetof(struct X3D_EspduTransform,isStandAlone));
3024 }
3025 if(pnode->isNetworkReader != isNetworkReader){
3026 pnode->isNetworkReader = isNetworkReader;
3027 MARK_EVENT(node,offsetof(struct X3D_EspduTransform,isNetworkReader));
3028 }
3029 if(pnode->isNetworkWriter != isNetworkWriter){
3030 pnode->isNetworkWriter = isNetworkWriter;
3031 MARK_EVENT(node,offsetof(struct X3D_EspduTransform,isNetworkWriter));
3032 }
3033 }
3034 break;
3035 default:
3036 break;
3037 }
3038}
3039
3040
3041int dis_read_stream(unsigned char * datastream, int streamsize, struct Vector *pdus, int *heard)
3042{
3043 int pdutype, bytesread;
3044 unsigned char *carat, *carat2;
3045 static char pdubuffer[10000];
3046 unsigned char *pdubuf;
3047 struct Pdu* pdu;
3048 bytesread = 0;
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){
3054 int j;
3055 printf("%d\t",i);
3056 for(j=0;j<10;j++){
3057 printf("%5d",(int)carat[i+j]);
3058 }
3059 printf("\n");
3060 }
3061 pdutype = (int)(carat[2]);
3062 distype = pduToDis(pdutype);
3063 //printf("pdu type=%d distype=%d",(int)pdutype, distype);
3064
3065 pdubuf = dis_ctor(distype);
3066 carat2 = dis_unmarshal(carat,pdubuf,distype);
3067 nbytes = (carat2 - carat);
3068 pdu = (struct Pdu*)pdubuf;
3069 pdu->padding = TAG_UNCLAIMED;
3070 //printf("un-marshed version %d pdutype= %d\n",pdu->protocolVersion,pdu->pduType);
3071 vector_pushBack(struct Pdu*,pdus,pdu);
3072 //printf("unmarshed bits %d bytes %d\n",nbytes*8,nbytes);
3073
3074 if(0){
3075 //try marshalling, then compare bytestreams
3076 unsigned char buf3[32000];
3077 unsigned char *carat3;
3078 int b3size;
3079 carat3 = dis_marshal(buf3,pdubuf,distype);
3080 b3size = carat3 - buf3;
3081 printf("marshed bits %d bytes %d\n",b3size*8,b3size);
3082
3083 if(memcmp(carat,buf3,b3size) == 0)
3084 printf("bravo\n");
3085 else{
3086 printf("youch\n");
3087 for(i=0;i<210;i+=10){
3088 int j;
3089 printf("%d\t",i);
3090 for(j=0;j<10;j++){
3091 printf("%5d",(int)buf3[i+j]);
3092 }
3093 printf("\n");
3094 }
3095 }
3096
3097 }
3098
3099
3100 if(0) if(pdutype == 1){
3101 int n;
3102 struct EntityStatePdu* p = (struct EntityStatePdu*)pdubuf;
3103 printf("loc %lf %lf %lf rot %f %f %f\n",
3106#ifdef DIS2012
3107 n = p->numberOfVariableParameters;
3108#else //DIS1998
3110#endif
3111 if(n){
3112 //unsigned char recordType;
3114 //double variableParameterFields1;
3116 //unsigned int variableParameterFields2;
3118 //unsigned short variableParameterFields3;
3120 //unsigned char variableParameterFields4;
3121 //struct VariableParameter *v;
3122
3123#ifdef DIS2012
3124 struct ArticulatedParts *v;
3125 v = (struct ArticulatedParts*)p->variableParameters;
3126 printf("v address = %p\n",v);
3127 //v = *vlist;
3128 for(i=0;i<n;i++){
3129 printf("%d %d %d %d %d %lf\n",
3130 i,
3131 (int)v[i].recordType,
3132 (int)v[i].changeIndicator,
3133 (int)v[i].partAttachedTo,
3134 (int)v[i].parameterType,
3135 v[i].parameterValue
3136 );
3137 }
3138#else //DIS1998
3139 struct ArticulationParameter *v;
3141 printf("v address = %p\n",v);
3142 //v = *vlist;
3143 for(i=0;i<n;i++){
3144 printf("%d %d %d %d %d %lf\n",
3145 i,
3146 (int)v[i].parameterTypeDesignator,
3147 (int)v[i].changeIndicator,
3148 (int)v[i].partAttachedTo,
3149 (int)v[i].parameterType,
3150 v[i].parameterValue
3151 );
3152 }
3153#endif
3154 }
3155 }
3156 //dis_dtor(pdubuf,distype);
3157 bytesread += nbytes;
3158 carat = carat2;
3159 //printf("bytes left = %d - %d = %d\n",streamsize, (int)(carat - datastream), streamsize - (int)(carat-datastream));
3160 //printf("\n");
3161 //if(0) for(i=0;i<npdus;i++){
3162 // if(registeredPdus[i]->pduType == pdutype){
3163 // //I think there should be more filtering here
3164 // //for example is it the right target IP + port + entityID?
3165 // memcpy(registeredPdus[i],pdubuffer,nbytes);
3166 // break;
3167 // }
3168 //}
3169 }
3170 return 0; //maybe an error number will be returned here in future
3171}
3172
3173int dis_write_stream(unsigned char * datastream, struct Vector *pdus)
3174{
3175 //missing maxsize on buffer
3176 int i, nbytes;
3177 unsigned char *carat;
3178 carat = datastream;
3179 nbytes = 0;
3180 if(pdus && pdus->n){
3181 for(i=0;i<pdus->n;i++){
3182 struct Pdu *pdu = vector_get(struct Pdu*,pdus,i);
3183 //printf("dis_wrt_str protocol %d pdutype %d\n",pdu->protocolVersion,pdu->pduType);
3184 int distype = pduToDis(pdu->pduType);
3185 carat = dis_marshal(carat,(unsigned char*)pdu,distype);
3186 //printf("pdu %d wrote %d bytes\n",i,nbytes);
3187 }
3188 // *streamsize = nbytes;
3189 nbytes = (int)(carat - datastream);
3190 }
3191 return nbytes;
3192}
3193
3194struct X3D_Node* findNodeByName(char* defname) {
3195 //its weird we don't have a function for this already,
3196 // some relating to parser, and some relating to EAI,
3197 // but we want something thats parser-agnostic and will go through context->defnames.
3198 struct X3D_Node* node, *root;
3199 struct X3D_Proto* context;
3200 node = NULL;
3201 root = rootNode();
3202 context = X3D_PROTO(root);
3203 struct brotoDefpair def;
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);
3208 //printf("%x %x %s\n",node,def.node,def.name);
3209 if (!strcmp(def.name,defname)) {
3210 node = def.node;
3211 break;
3212 }
3213 }
3214 }
3215 return node;
3216}
3217/* using regular pdu2espdu
3218struct Vector* avatars = NULL; //2023 multiplayer
3219int dis_pdus2avatars(struct Vector* pdus) {
3220 //2023 multiplayer
3221 //update avatars of other players, which may involve
3222 // adding an avatar, when a new player joins, with a certain appearance
3223 // removing an avatar (heartbeat or time since last update > participation_threshold_time ie 5 minutes)
3224 // updating avatar pose in world coords
3225 // updating avatar articulation, such as walking, standing, reaching - could there be flag combos?
3226 int i, ihit;
3227 struct Pdu* pdu;
3228
3229 ihit = 0;
3230 if (!pdus) return ihit;
3231 for (i = 0; i < pdus->n; i++)
3232 {
3233 pdu = vector_get(struct Pdu*, pdus, i);
3234 if(pdu->padding == TAG_UNCLAIMED)
3235 switch (pdu->pduType) {
3236 case PDU_ENTITY_STATE:
3237 {
3238 //ENTITYSTATE category 77 avatar
3239 //we assume ENTITY_STATE pdus with category 77 are avatar updates
3240
3241 struct EntityStatePdu* espdu;
3242 espdu = (struct EntityStatePdu*)pdu;
3243 if (espdu->entityID.application == fwl_get_DISapplication()
3244 && espdu->entityID.site == fwl_get_DISsite()) {
3245 pdu->padding = TAG_SAME_PROGRAM;
3246 ihit++;
3247 break;
3248 }
3249 if (espdu->entityType.category != 77) {
3250 // not avatar, remains unclaimed
3251 break;
3252 }
3253 static struct X3D_Group* avatar_group = NULL;
3254 if(!avatar_group) avatar_group = (struct X3D_Group*)findNodeByName("AvatarHolder");
3255 if (!avatar_group) {
3256 printf("your scene needs a Group DEF AvatarHolder\n");
3257 break;
3258 }
3259 struct Multi_Node* avatars = &avatar_group->children;
3260 struct X3D_EspduTransform* pnode, * tnode;
3261 pnode = NULL;
3262 for (int j = 0; j < avatars->n; j++) {
3263 // 2018.pdf 6.2.80.3 Application Number implied an instance number,
3264 // so should not be same unless loopback testing
3265 struct X3D_EspduTransform* tnode = (struct X3D_EspduTransform*)avatars->p[j];
3266 int OK = TRUE;
3267 //when sending avatars, we set entityID = sending programID
3268 if (espdu->entityID.entity != tnode->entityID) OK = FALSE;
3269 if (OK) {
3270 pnode = tnode;
3271 //still TAG_UNCLAIMED
3272 break;
3273 }
3274 }
3275 if (!pnode) break;
3276 ihit++;
3277 pdu->padding = TAG_AVATAR;
3278 pnode->_change++; //mark node changed
3279 pnode->timestamp = TickTime();
3280
3281 if (pnode->__geoSystem) {
3282 Quaternion qgc2tcs, qtcs2body, qgc2body;
3283 struct SFVec3d gd, gc, translate;
3284 struct SFVec4d rotate;
3285 double localxyz[3], tcsxyz[3], tcs2bodyxyz[3], world2bodyxyz[3];
3286 float xyza[4];
3287 Geosys* gs;
3288 gs = GEOSYS(pnode->__geoSystem);
3289 user2gc(gs, &pnode->geoCoords, 1, &gc);
3290 //gc2gd(gs,&gc,1,&gd);
3291 //gc2tcs_transform(gs,&gd,&translate,&rotate);
3292 gc2tcsB_transform(gs, &gc, &translate, &rotate);
3293 //somehow get body/entity into world/gc - rotation and translation
3294 {
3295 //rotation
3296 //assumption (Apr 2018 don't know how Xj3d does it, here's dug9's guess):
3297 // pdu is world2body
3298 // espdutransform.rotation = local2body
3299 // - where local is TCS Topocentric Coord System as described for GeoLocation
3300 // - and world is GC
3301 // local2body = world2local.inverse x world2body
3302 // for freewrl world2local is gc2tcs
3303 Quaternion qtcs2gc, q;
3304 float ypr[3], xyza[4];
3305 ypr[0] = -espdu->entityOrientation.psi;
3306 ypr[1] = espdu->entityOrientation.theta;
3307 ypr[2] = espdu->entityOrientation.phi;
3308 ypr2axisangle(ypr, xyza);
3309 xyza[3] = -xyza[3];
3310 //vecprint4fb("recv xyza",xyza,"\n");
3311
3312 vrmlrot4f_to_quaternion(&qgc2body, xyza);
3313 vrmlrot4d_to_quaternion(&qgc2tcs, rotate.c);
3314 quaternion_inverse(&qtcs2gc, &qgc2tcs);
3315 quaternion_multiply(&qtcs2body, &qtcs2gc, &qgc2body);
3316 quaternion_set(&q, &qtcs2body);
3317 quaternion_to_vrmlrot4f(&q, pnode->rotation.c);
3318 }
3319 {
3320 //translation - dug9 debate: could do it one of 2 ways
3321 enum transmethod {
3322 TRANS_ZERO = 1,
3323 TRANS_LOCATION_MINUS_GEOCOORD = 2
3324 };
3325 //static int transmethod = TRANS_LOCATION_MINUS_GEOCOORD;
3326 static int transmethod = TRANS_ZERO;
3327
3328 vector3double2vec3d(world2bodyxyz, &espdu->entityLocation);
3329 if (transmethod == TRANS_LOCATION_MINUS_GEOCOORD) {
3330 //METHOD 1: translation = Location - geoCoords
3331 struct SFVec3d world, tcs;
3332 veccopyd(world.c, world2bodyxyz);
3333 //gc2tcs(gs,&gd,&world,1,&tcs);
3334 gc2tcsB(gs, &gc, &world, 1, &tcs);
3335 double2float(pnode->translation.c, tcs.c, 3);
3336 }
3337 else {
3338 //TRANS_ZERO
3339 //METHOD 2: geoCoords = Location; translation = 000
3340 // x smoothing doesn't work if done in translation / tcs space
3341 struct SFVec3d world, tcs2, tcs1;
3342 double deltatcs[3];
3343 float deltap[3];
3344 static int want_smoothing = 1;
3345 if (want_smoothing) {
3346 //gc2tcs(gs,&gd,&gc,1,&tcs1);
3347 gc2tcsB(gs, &gc, &gc, 1, &tcs1);
3348 veccopyd(world.c, world2bodyxyz);
3349 gc2tcs(gs, &gd, &world, 1, &tcs2);
3350 gc2tcsB(gs, &gc, &world, 1, &tcs2);
3351 vecdifd(deltatcs, tcs1.c, tcs2.c);
3352 double2float(deltap, deltatcs, 3);
3353 vecadd3f(pnode->_p0.c, pnode->_p0.c, deltap);
3354 }
3355 gc2user(gs, &world, 1, &pnode->geoCoords);
3356 //node->_change++;
3357 vecset3f(pnode->translation.c, 0.0f, 0.0f, 0.0f);
3358
3359 }
3360 }
3361 // recv geo dead reckoning
3362 {
3363 float V[3], A[3];
3364 // in entity or world, depending on drmethod
3365 vector3float2vec3f(V, &espdu->entityLinearVelocity);
3366 veccopy3f(pnode->linearVelocity.c, V);
3367
3368 }
3369
3370 }
3371 else {
3372 //non-geosystem scene. Apr 22, 2018 we aren't using this now
3373 // -- everything goes through geosystem code above
3374 // -- but keeping this until we benchmark against Brutzman
3375 //translation - assumes companion scenes will have same parent transform stack
3376 //(x, -z, y).
3377 pnode->translation.c[0] = espdu->entityLocation.x;
3378 pnode->translation.c[1] = espdu->entityLocation.z;
3379 pnode->translation.c[2] = -espdu->entityLocation.y;
3380 //rotation
3381 if (0) {
3382 Quaternion qA;
3383 float ypr[3];
3384 double r[4];
3385 float* c = pnode->rotation.c;
3386 ypr[0] = espdu->entityOrientation.phi;
3387 ypr[1] = espdu->entityOrientation.psi;
3388 ypr[2] = espdu->entityOrientation.theta;
3389 euler2quat(&qA, ypr[0], ypr[1], ypr[2]);
3390 //quaternion_normalize(&qA);
3391 //vrmlrot_to_quaternion(&qA,c[0],c[1],c[2],c[3]);
3392 quaternion_to_vrmlrot(&qA, &r[0], &r[1], &r[2], &r[3]);
3393 c[0] = (float)r[0];
3394 c[1] = (float)r[1];
3395 c[2] = (float)r[2];
3396 c[3] = (float)r[3];
3397 }
3398 if (1) {
3399 float ypr[3];
3400 ypr[0] = -espdu->entityOrientation.psi; //gimbal.js shows -yaw
3401 ypr[1] = espdu->entityOrientation.theta;
3402 ypr[2] = espdu->entityOrientation.phi;
3403 ypr2axisangle(ypr, pnode->rotation.c);
3404 }
3405 // dead reckoning
3406 vector3float2vec3f(pnode->linearVelocity.c, &espdu->entityLinearVelocity);
3407
3408 }
3409 //articuation parameters
3410 pnode->articulationParameterArray.n = espdu->numberOfArticulationParameters;
3411 //printf("recv art count %d\n",espdu->numberOfArticulationParameters);
3412 if (pnode->articulationParameterArray.n) {
3413 struct ArticulationParameter* ap;
3414 float* pp;
3415 int i, np = pnode->articulationParameterArray.n;
3416 ap = espdu->articulationParameters;
3417 pp = malloc(np * sizeof(float));
3418 //printf("received %d articulation parameters:\n",np);
3419 for (i = 0; i < np; i++) {
3420 //ap[i].parameterTypeDesignator = 0; //0 is articulated part
3421 //ap[i].parameterType = 1029; //1024 - rudder + 5 X
3422 pp[i] = (float)ap[i].parameterValue;
3423 //printf("%d %f\n",i,pp[i]);
3424 //ap[i].partAttachedTo = 0;
3425 switch (i) {
3426 case 0: pnode->articulationParameterValue0_changed = pp[i]; break;
3427 case 1: pnode->articulationParameterValue1_changed = pp[i]; break;
3428 case 2: pnode->articulationParameterValue2_changed = pp[i]; break;
3429 case 3: pnode->articulationParameterValue3_changed = pp[i]; break;
3430 case 4: pnode->articulationParameterValue4_changed = pp[i]; break;
3431 case 5: pnode->articulationParameterValue5_changed = pp[i]; break;
3432 case 6: pnode->articulationParameterValue6_changed = pp[i]; break;
3433 case 7: pnode->articulationParameterValue7_changed = pp[i]; break;
3434 default:
3435 break;
3436 }
3437 }
3438 if (pnode->articulationParameterArray.p) free(pnode->articulationParameterArray.p);
3439 pnode->articulationParameterArray.p = pp;
3440 //done in generic mark_changed_fields //MARK_EVENT(X3D_NODE(pnode),offsetof(struct X3D_EspduTransform,articulationParameterArray));
3441 }
3442 pnode->_pduchange_es = TRUE;
3443 if (espdu->entityAppearance | 1 << 20) {
3444 //http://movesinstitute.org/~mcgredo/MV3500/hla/1278.1-200X%20Draft%2016%20rev%2018.pdf
3445 //p.50 no dead reckoning if isFrozen bit is set, bit 21 of entityAppearance
3446 //(why can't they just leave dead reckoning parameters 0, and run through formula? H: specs written in 1990s for 80386 processors)
3447 //pnode->_isFrozen = TRUE; //pduchange_es = FALSE;
3448 }
3449
3450 }
3451 }
3452
3453 }
3454 return ihit;
3455}
3456*/
3457
3458static double lasttime;
3459static char buf[32768];
3460static struct Vector *pdus = NULL;
3461
3462void dis_recvloop(){
3463 //there are a few ways to do non-blocking recv
3464 //1. ioctlsocket non-blocking - set socket to not block
3465 //2. select() - select itself blocks, so should have its own thread
3466 //3. PEEK flag in recvfrom
3467 //Oct 24, 2017 choice: 1.
3468 // - because we aren't doing a separate thread yet, so 1 or 3, and 3 worked when tried first
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);
3476
3477 //since not select()ing we have to check all sockets (if readInterval?)
3478 //for 'Entity Discovery' at least one DIS node needs to be in scene, with IP/port to check
3479 for(i=0;i<sockets_recv->n;i++){
3480 struct dis_socket *dsock = vector_get_ptr(struct dis_socket,sockets_recv,i);
3481 //things may have built up in the input socket, so we loop till flushed
3482 do{
3483 struct X3D_DISEntityManager* sockem = NULL;
3484 heard = FALSE;
3485 more = FALSE;
3486 nbytes = sockrecvfrom(dsock,buf,32000);
3487 if(nbytes > 0){
3488 int nhit = 0;
3489 more = TRUE;
3490 dsock->lasttime = thistime;
3491 //printf("sock read nbytes = %d\n",nbytes);
3492 //free last round
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));
3496 }
3497 pdus->n = 0;
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);
3501 pdu->padding = TAG_UNCLAIMED; //0
3502 }
3503 //print some stuff to the console, to prove we got a state update
3504 //printf("hallelluha %d\n",count++);
3505 //check pdus against all nodes registered on the port
3506 // in case the message is for an existing node
3507
3508 int ihit;
3509 if(dsock->registered){
3510 //printf("dsock.registered.n %d\n", dsock->registered->n);
3511 for(j=0;j<dsock->registered->n;j++){
3512 struct X3D_Node *node = vector_get(struct X3D_Node*,dsock->registered,j);
3513 //check site and application ID
3514 //distribute to registered nodes by entityID
3515 ihit = 0;
3516 switch(node->_nodeType){
3517 case NODE_EspduTransform:
3518 ihit = dis_pdus2node_espdu(node, pdus);
3519 break;
3520 case NODE_DISEntityManager:
3521 sockem = (struct X3D_DISEntityManager*) node;
3522 ihit = dis_pdus2node_sm(node, pdus);
3523 break;
3524 case NODE_ReceiverPdu:
3525 ihit = dis_pdus2node_receiver(node, pdus);
3526 break;
3527 case NODE_TransmitterPdu:
3528 ihit = dis_pdus2node_transmitter(node, pdus);
3529 break;
3530 case NODE_SignalPdu:
3531 ihit = dis_pdus2node_signal(node, pdus);
3532 break;
3533 default:
3534 break;
3535 }
3536 if(ihit){
3537 if(heard) set_rtp_heard(node);
3538 dis_set_isActive(node,TRUE);
3539 dis_set_node_lasttime(node,thistime);
3540 nhit += ihit;
3541 }
3542 }
3543 }
3544 //2023 multiplayer>>
3545 ihit = dis_pdus2sensors(pdus);
3546 nhit += ihit;
3547 //<<2023 multiplayer
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);
3551 counts[pdu->padding]++;
3552 }
3553 //TAG_UNCLAIMED = 0
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]);
3556 if(nhit < pdus->n){
3557 // any 'left-over' pdus might be 'entity discovery' candidates
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);
3561 }
3562 }
3563 }while(more);
3564 if(dsock->registered){
3565#define RETIRE_TIME 300.0 //5 MINUTES?
3566 //check if any node listeners have gone inactive
3567 //2023 multiplayer sensors don't go stale. avatars do
3568 struct X3D_DISEntityManager* sockem = NULL;
3569 for(j=0;j<dsock->registered->n;j++){
3570 struct X3D_Node *node = vector_get(struct X3D_Node*,dsock->registered,j);
3571 if(node->_nodeType == NODE_DISEntityManager){
3572 sockem = (struct X3D_DISEntityManager*)node;
3573 }
3574 if(sockem) break;
3575 }
3576 for (j = 0; j < dsock->registered->n; j++) {
3577 //update isActive
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);
3582 //printf("%d %s %lf %lf %lf %lf setting isActive false\n", j, stringNodeType(node->_nodeType), readinterval, writeinterval, lasttime, thistime);
3583 double timeout = RETIRE_TIME;
3584 if (node->_nodeType == NODE_EspduTransform) {
3585 struct X3D_EspduTransform* espdu = (struct X3D_EspduTransform*)node;
3586 if (!espdu->isNetworkWriter) {
3587 timeout = 5.0;
3588 }
3589 }
3590 if (thistime - lasttime > timeout) {
3591 //5 second rule: if a node recvs nothing for 5 seconds, turn isActive to FALSE.
3592 dis_set_isActive(node, FALSE);
3593 }
3594 //if its been several (?) heartbeat increments since we last heard from an entity
3595 // the DIS specs talk about removing (opposite of adding by 'entity discovery')
3596 if (thistime - lasttime > (timeout * 3)) {
3597 //if in entitymanager state.entities, removeChildren
3598 int ihit = 0;
3599 if (sockem && node->_nodeType == NODE_EspduTransform) {
3600 ihit = dis_entity_retire(sockem, node);
3601 }
3602 if (ihit == 1) {
3603 printf(" retired one\n");
3604 //printf("thisttime %lf lasttime %lf\n",thistime,lasttime);
3605 }
3606 //if(ihit == -1) printf(" cetiree not in EM list\n");
3607 }
3608 }
3609 }
3610 if(sockem && sockem->removedEntities.n) {
3611 //printf("removedEntities.n=%d\n",node->removedEntities.n);
3612 MARK_EVENT(X3D_NODE(sockem),offsetof(struct X3D_DISEntityManager,removedEntities));
3613 }
3614
3615 }
3616 }
3617
3618}
3619
3620void dis_open_socket(struct dis_socket* dsock){
3621 if(dsock->multicastRelayHost && strlen(dsock->multicastRelayHost)){
3622 printf("[%s]\n",dsock->multicastRelayHost);
3623 printf("\n");
3624 //direct socket
3625 }else{
3626 socket_open(dsock);
3627 }
3628
3629}
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){
3633 void *preg; //something to store in the node, to say which socket its registerd in
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;
3639 preg = NULL;
3640 if(inetworkmode == 0) preg = NULL; //not joined/registered to any socket
3641 if(inetworkmode == 1){
3642 int i, j, ifound;
3643 struct dis_socket *dsock;
3644 if(!sockets_recv) sockets_recv = newVector(struct dis_socket,10);
3645 //if theres a (networkMode,address,port) tuple already registered, then we join
3646 ifound = -1;
3647 dsock = NULL;
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){
3651 //if(dsock->registered){
3652 // for(j=0;j<dsock->registered->n;j++){
3653 // we assume its not registered
3654 // }
3655 //}
3656 ifound = i;
3657 break;
3658 }
3659 }
3660 if(ifound == -1){
3661 //create a socket
3662 struct dis_socket asock;
3663 memset(&asock,0,sizeof(struct dis_socket));
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;
3671 //open port
3672 dis_open_socket(dsock);
3673 }
3674 //join socket
3675 if(!dsock->registered) dsock->registered = newVector(struct X3D_Node*,20);
3676 vector_pushBack(struct X3D_Node*,dsock->registered,node);
3677 preg = (void*)dsock;
3678 }
3679 if(inetworkmode==2){
3680 int i, j, ifound;
3681 struct dis_socket *dsock;
3682 if(!sockets_send) sockets_send = newVector(struct dis_socket,10);
3683 //if theres a (networkMode,address,port) tuple already registered, then we join
3684 ifound = -1;
3685 dsock = NULL;
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){
3689 //if(dsock->registered){
3690 // for(j=0;j<dsock->registered->n;j++){
3691 // we assume its not registered
3692 // }
3693 //}
3694 ifound = i;
3695 break;
3696 }
3697 }
3698 if(ifound == -1){
3699 //create a socket
3700 struct dis_socket asock;
3701 memset(&asock,0,sizeof(struct dis_socket));
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;
3709 //open port
3710 dis_open_socket(dsock);
3711 }
3712 //join socket
3713 if(!dsock->registered) dsock->registered = newVector(struct X3D_Node*,20);
3714 vector_pushBack(struct X3D_Node*,dsock->registered,node);
3715 preg = (void*)dsock;
3716 }
3717 dis_set_isNetworkMode(node, inetworkmode);
3718 return preg;
3719}
3720void dis_unregister(struct dis_socket * dsock, struct X3D_Node* node){
3721 //unregister / un-join node from socket
3722 int j;
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);
3727 break;
3728 }
3729 }
3730 }
3731}
3732int dis_check_socket_change(struct dis_socket* dsock,char *address, int port,
3733 char *multicastRelayHost, int multicastRelayPort, char *networkMode){
3734 //do the node fields still jive/match with the socket it joined?
3735 //(a weakness of the freewrl architecture: there's no per-field change flag
3736 //so if a node with a zillion fields is flagged as changed, we have to
3737 //re-'compile' the whole node, or save old values in _old fields on the node for comparison,
3738 //or (more normally) MARK_EVENT(node,offset) which runs through lists of registered routes.
3739 //here we are comparing a few fields with 'what they must have been when registered')
3740 //IDEA: save a duplicate of the node in _oldNode field, so can compare 1:1 with any field
3741
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; //not registered, and no need to register
3748 if(dsock == NULL) return TRUE; //need to register
3749 if(inetworkmode != dsock->idir) return TRUE; //change of direction
3750 if(strcmp(address,dsock->address)) return TRUE; //change of address
3751 if(sport != dsock->port) return TRUE; //change of port
3752 if(strcmp(multicastRelayHost,dsock->multicastRelayHost)) return TRUE;
3753 if(multicastRelayPort != dsock->multicastRelayPort) return TRUE;
3754
3755 return FALSE;
3756}
3757// freewrl problem: _changed flag is per-node
3758// x but its bad DIS ettiquette to resend pdus that haven't changed
3759// to detect per-pdu changes:
3760// 1. during node_compile
3761// 1.a do once: create node->_oldstate and register for disposal, copy node to _oldstate
3762// 1.b compare _oldState and node
3763// - compare fields per-pdu, and flag per-pdu
3764// common flags:
3765// _pduchange_networksensor
3766// per-pdu flags:
3767// espdu
3768// _pduchange_deadreckoning
3769// _pduchange_articulationparameters
3770// _pduchange_collision
3771// _pduchange_fire
3772// _pduchange_detonation
3773// recieverpdu
3774// _pduchange_receiver
3775// signalpdu
3776// _pduchange_signal
3777// transmitterpdu
3778// _pduchange_transmitter
3779// 1.c copy node fields to _oldState
3780// 1.d mark node compiled
3781// 2. in dis_sendloop only send pdus that changed
3782void shallow_copy_node(struct X3D_Node *copy, struct X3D_Node *original )
3783{
3784 //we just want to copy the public fields, not our private _ fields
3785 // which include things like _pduchange_espdutransform etc
3786 // same for later when we compare, just the public fields
3787 const int *offset;
3788 unsigned char *src, *dest;
3789 src = (unsigned char *)original;
3790 dest = (unsigned char *)copy;
3791
3792 offset = NODE_OFFSETS[original->_nodeType];
3793 while(offset[0] > -1){
3794 if(offset[4] > 0){
3795 //offset[4] is the specs attribute, and if its a private field ie _name then it should have 0
3796 // we just want the public fields here
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);
3801 }
3802 offset += 6;
3803 };
3804}
3805int shallow_compare_field(int typeIndex, union anyVrml* source, union anyVrml* dest)
3806{
3807 int i, isize, has_changed;
3808 int sftype, isMF;
3809 struct Multi_Node *mfs,*mfd;
3810 has_changed = FALSE;
3811
3812 isMF = typeIndex % 2;
3813 sftype = typeIndex - isMF;
3814 //from EAI_C_CommonFunctions.c
3815 //isize = returnElementLength(sftype) * returnElementRowSize(sftype);
3816 isize = sizeofSForMF(sftype);
3817 if(isMF)
3818 {
3819 int nele;
3820 char *ps, *pd;
3821 mfs = (struct Multi_Node*)source;
3822 mfd = (struct Multi_Node*)dest;
3823 //self assignment is no-op
3824 if(mfs->n != mfd->n){
3825 has_changed = TRUE;
3826 }else{
3827 ps = (char *)mfs->p;
3828 pd = (char *)mfd->p;
3829 for(i=0;i<mfs->n;i++)
3830 {
3831 has_changed = shallow_compare_field(sftype,(union anyVrml*)ps,(union anyVrml*)pd);
3832 ps += isize;
3833 pd += isize;
3834 }
3835 }
3836 }else{
3837 //isSF
3838 switch(typeIndex)
3839 {
3840 case FIELDTYPE_SFString:
3841 {
3842 //go deep, same as copy_field
3843 struct Uni_String **ss, **sd;
3844 if(source != dest){
3845 has_changed = TRUE;
3846 }else{
3847 ss = (struct Uni_String **)source;
3848 sd = (struct Uni_String **)dest;
3849 if(*ss && *sd){
3850 has_changed = memcmp(*sd,*ss,sizeof(struct Uni_String)) ? TRUE : FALSE;
3851 }
3852 }
3853 }
3854 break;
3855 default:
3856 //memcpy(dest,source,sizeof(union anyVrml));
3857 has_changed = memcmp(dest,source,isize) ? TRUE : FALSE;
3858 break;
3859 }
3860 }
3861 return has_changed;
3862} //return copy_field
3863
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;
3867 int k, has_changed;
3868
3869 src = (unsigned char *)old;
3870 dest = (unsigned char *)node;
3871 fname = PFIELDS;
3872 k = 0;
3873 has_changed = 0;
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);
3882 break;
3883 }
3884 offset += 6;
3885 };
3886 k++;
3887 };
3888 return has_changed ? TRUE : FALSE;
3889}
3890
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;
3894 int k, count;
3895
3896 src = (unsigned char *)old;
3897 dest = (unsigned char *)node;
3898 fname = PFIELDS;
3899 k = 0;
3900 count = 0;
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]);
3910 count++;
3911 }
3912 break;
3913 }
3914 offset += 6;
3915 };
3916 k++;
3917 };
3918 return count;
3919}
3920
3921//here are some per-pdu lists of public fields, useful for detecting per-pdu field changes
3922
3923const int FIELDS_networksensor [] = {
3924 FIELDNAMES_enabled,
3925 FIELDNAMES_isActive,
3926 FIELDNAMES_timestamp,
3927 FIELDNAMES_address,
3928 FIELDNAMES_port,
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,
3939 //FIELDNAMES__registered, //not the private fields
3940 //FIELDNAMES__dsock,
3941 //FIELDNAMES__lasttime,
3942 -1,
3943};
3944
3945const int FIELDS_entity [] = {
3946 FIELDNAMES_entityID,
3947 FIELDNAMES_applicationID,
3948 FIELDNAMES_siteID,
3949 -1,
3950};
3951
3952const int FIELDS_geo [] = {
3953 FIELDNAMES_geoSystem,
3954 FIELDNAMES_geoCoords,
3955 -1,
3956};
3957const int FIELDS_geosys [] = {
3958 FIELDNAMES_geoSystem,
3959 -1,
3960};
3961const int FIELDS_geocoord [] = {
3962 FIELDNAMES_geoCoords,
3963 -1,
3964};
3965
3966
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,
3975 -1,
3976};
3977const int FIELDS_create [] = {
3978 FIELDNAMES_addedEntities,
3979 -1,
3980};
3981const int FIELDS_remove [] = {
3982 FIELDNAMES_removedEntities,
3983 -1,
3984};
3985
3986const int FIELDS_es_force [] = {
3987 FIELDNAMES_forceID,
3988 //FIELDNAMES_marking,
3989 -1,
3990};
3991
3992const int FIELDS_es_transform [] = {
3993 FIELDNAMES_center,
3994 FIELDNAMES_children,
3995 FIELDNAMES_rotation,
3996 FIELDNAMES_scale,
3997 FIELDNAMES_scaleOrientation,
3998 FIELDNAMES_translation,
3999 //FIELDNAMES_bboxCenter,
4000 //FIELDNAMES_bboxSize,
4001 -1,
4002};
4003
4004
4005const int FIELDS_es_deadreckoning [] = {
4006 FIELDNAMES_deadReckoning,
4007 FIELDNAMES_linearVelocity,
4008 FIELDNAMES_linearAcceleration,
4009 -1,
4010};
4011
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,
4035 -1,
4036};
4037
4038const int FIELDS_collision [] = {
4039 FIELDNAMES_collisionType,
4040 FIELDNAMES_collideTime,
4041 FIELDNAMES_isCollided,
4042 -1,
4043};
4044
4045const int FIELDS_events [] = {
4046 FIELDNAMES_eventEntityID,
4047 FIELDNAMES_eventApplicationID,
4048 FIELDNAMES_eventSiteID,
4049 FIELDNAMES_eventNumber,
4050 -1,
4051};
4052
4053const int FIELDS_fire [] = {
4054 FIELDNAMES_fired1,
4055 FIELDNAMES_fired2,
4056 FIELDNAMES_fireMissionIndex,
4057 FIELDNAMES_firingRange,
4058 FIELDNAMES_firedTime,
4059 -1,
4060};
4061
4062const int FIELDS_detonation [] = {
4063 FIELDNAMES_detonationLocation,
4064 FIELDNAMES_detonationRelativeLocation,
4065 FIELDNAMES_detonationResult,
4066 FIELDNAMES_detonateTime,
4067 FIELDNAMES_isDetonated,
4068 -1,
4069};
4070
4071const int FIELDS_munition [] = {
4072 FIELDNAMES_munitionEntityID,
4073 FIELDNAMES_munitionApplicationID,
4074 FIELDNAMES_munitionSiteID,
4075 FIELDNAMES_munitionStartPoint,
4076 FIELDNAMES_munitionEndPoint,
4077 FIELDNAMES_munitionQuantity,
4078 -1,
4079};
4080const int FIELDS_rate [] = {
4081 FIELDNAMES_firingRate,
4082 FIELDNAMES_fuse,
4083 FIELDNAMES_warhead,
4084 -1,
4085};
4086
4087const int FIELDS_receiver [] = {
4088 FIELDNAMES_radioID,
4089 FIELDNAMES_whichGeometry,
4090 FIELDNAMES_receiverState,
4091 FIELDNAMES_receivedPower,
4092 FIELDNAMES_transmitterEntityID,
4093 FIELDNAMES_transmitterApplicationID,
4094 FIELDNAMES_transmitterSiteID,
4095 FIELDNAMES_transmitterRadioID,
4096 -1,
4097};
4098
4099const int FIELDS_signal [] = {
4100 FIELDNAMES_radioID,
4101 FIELDNAMES_whichGeometry,
4102 FIELDNAMES_data,
4103 FIELDNAMES_dataLength,
4104 FIELDNAMES_encodingScheme,
4105 FIELDNAMES_sampleRate,
4106 FIELDNAMES_samples,
4107 FIELDNAMES_tdlType,
4108 -1,
4109};
4110
4111const int FIELDS_transmitter [] = {
4112 FIELDNAMES_radioID,
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,
4126 FIELDNAMES_power,
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,
4137 -1,
4138};
4139
4140void compile_DIS_network(struct X3D_EspduTransform *node){
4141 if(node->_oldState == NULL){
4142 //change detection
4143 //later we'll copy the entire node after we detect any changed fields
4144 struct X3D_Node *old;
4145 old = createNewX3DNode0(node->_nodeType);
4146 //shallow_copy_node(old,X3D_NODE(node));
4147 node->_oldState = old; //I think one underscore means dispose
4148 }
4149 if(!node->_registered){
4150 void *psock;
4151 node->address->strptr = strdup( fwl_get_DISaddress() ? fwl_get_DISaddress() : node->address->strptr );
4152 //if (!strcmp(node->address->strptr, "localhost")) {
4153 // //node->address->strptr = "127.0.0.1";
4154 // struct hostent* hp = gethostbyname(node->address->strptr);
4155 // printf("official host name %s\n", hp->h_name);
4156 //}
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;
4161 //printf("address %s port %d site %d app %d\n", node->address->strptr, node->port, node->siteID, 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,
4168 node->siteID,
4169 node->writeInterval);
4170 node->_registered = TRUE;
4171 node->_dsock = psock;
4172 }
4173 if(node->_registered){
4174 //almost every field is [in,out] so can be changed at runtime
4175 //IDEA: save duplicate of nodetype in _oldnode field
4176 if(shallow_compare_node_fields(X3D_NODE(node),node->_oldState,FIELDS_networksensor)){
4177 int changed;
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);
4180 if(changed){
4181 dis_unregister((struct dis_socket*)node->_dsock,X3D_NODE(node));
4182 node->_registered = FALSE;
4183 node->_dsock = NULL;
4184 }
4185 }
4186 }
4187}
4188void compile_DIS_geo(struct X3D_EspduTransform *node){
4189 //Apr 2018 interpretation of geoSystem/geoCoords for DIS:
4190 //- world2body = world2tcs + tcs2body where tcs2body == translation
4191 // Scene
4192 // geoCoords used like GeoLocation, to convert ordinary nodes to geospatial
4193 // transform using DIS
4194 // children
4195 if(TRUE){
4196 //if(veclengthd(node->geoCoords.c) != 0.0){
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);
4200 }
4201 }
4202}
4203void compile_DIS_common(struct X3D_EspduTransform *node){
4204 //INITIALIZE_EXTENT;
4205 compile_DIS_network(node);
4206 compile_DIS_geo(node);
4207}
4208void compile_DIS_common_OLD(struct X3D_EspduTransform *node){
4209 if(node->_oldState == NULL){
4210 //change detection
4211 //later we'll copy the entire node after we detect any changed fields
4212 struct X3D_Node *old;
4213 old = createNewX3DNode0(node->_nodeType);
4214 //shallow_copy_node(old,X3D_NODE(node));
4215 node->_oldState = old; //I think one underscore means dispose
4216 }
4217 if(!node->_registered){
4218 void *psock;
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;
4224 }
4225 if(node->_registered){
4226 //almost every field is [in,out] so can be changed at runtime
4227 //IDEA: save duplicate of nodetype in _oldnode field
4228 if(shallow_compare_node_fields(X3D_NODE(node),node->_oldState,FIELDS_networksensor)){
4229 int changed;
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);
4232 if(changed){
4233 dis_unregister((struct dis_socket*)node->_dsock,X3D_NODE(node));
4234 node->_registered = FALSE;
4235 node->_dsock = NULL;
4236 }
4237 }
4238 }
4239 //Apr 2018 interpretation of geoSystem/geoCoords for DIS:
4240 //- world2body = world2tcs + tcs2body where tcs2body == translation
4241 // Scene
4242 // geoCoords used like GeoLocation, to convert ordinary nodes to geospatial
4243 // transform using DIS
4244 // children
4245 if(TRUE){
4246 //if(veclengthd(node->geoCoords.c) != 0.0){
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);
4250 }
4251 }
4252}
4253// >> RADIO
4254void compile_TransmitterPdu0(struct X3D_TransmitterPdu *node){
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;
4260 }
4261 }
4262 freeMallocedNodeFields(node->_oldState);
4263 shallow_copy_node(node->_oldState,X3D_NODE(node));
4264}
4265void compile_SignalPdu0(struct X3D_SignalPdu *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;
4271 }
4272 }
4273 freeMallocedNodeFields(node->_oldState);
4274 shallow_copy_node(node->_oldState,X3D_NODE(node));
4275}
4276void compile_ReceiverPdu0(struct X3D_ReceiverPdu *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;
4282 }
4283 }
4284 freeMallocedNodeFields(node->_oldState);
4285 shallow_copy_node(node->_oldState,X3D_NODE(node));
4286}
4287// << RADIO
4288
4289void compile_EspduTransform0(struct X3D_EspduTransform *node){
4290 //we use the same _pduchange flags and _oldState for both receiving and sending
4291 // but could be split if needed
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);
4298 //mark_changed_node_fields(X3D_NODE(node), node->_oldState, FIELDS_es_transform);
4299 }
4300 if(node->_pduchange_collision){
4301 mark_changed_node_fields(X3D_NODE(node), node->_oldState, FIELDS_collision);
4302 }
4303 if(node->_pduchange_fire){
4304 mark_changed_node_fields(X3D_NODE(node), node->_oldState, FIELDS_fire);
4305 }
4306 if(node->_pduchange_fire || node->_pduchange_collision){
4307 //mark_changed_node_fields(X3D_NODE(node), node->_oldState, FIELDS_events);
4308 }
4309 if(node->_pduchange_detonation){
4310 mark_changed_node_fields(X3D_NODE(node), node->_oldState, FIELDS_detonation);
4311 }
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);
4315 }
4316
4317 reset_node_pduchanged(X3D_NODE(node));
4318
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)){
4323 es_info = TRUE;
4324 }
4325 if(shallow_compare_node_fields(X3D_NODE(node),node->_oldState,FIELDS_es_force)){
4326 es_force = TRUE;
4327 }
4328 if(shallow_compare_node_fields(X3D_NODE(node),node->_oldState,FIELDS_es_deadreckoning)){
4329 es_deadreckoning = FALSE; //we'll do it elsewhere TRUE;
4330 }
4331 if(shallow_compare_node_fields(X3D_NODE(node),node->_oldState,FIELDS_es_articulation)){
4332 int i,n;
4333 struct X3D_EspduTransform *old = (struct X3D_EspduTransform *)node->_oldState;
4334 es_articulation = TRUE;
4335 node->articulationParameterArray.p = realloc(node->articulationParameterArray.p,16*sizeof(float));
4336 n = node->articulationParameterArray.n;
4337 for(i=0;i<8;i++){
4338 switch(i){
4339 case 0:
4340 if(node->set_articulationParameterValue0 != old->set_articulationParameterValue0)
4341 node->articulationParameterArray.p[i] = node->set_articulationParameterValue0;
4342 n=max(n,i);
4343 break;
4344 case 1:
4345 if(node->set_articulationParameterValue0 != old->set_articulationParameterValue1)
4346 node->articulationParameterArray.p[i] = node->set_articulationParameterValue1;
4347 n=max(n,i);
4348 break;
4349 case 2:
4350 if(node->set_articulationParameterValue0 != old->set_articulationParameterValue2)
4351 node->articulationParameterArray.p[i] = node->set_articulationParameterValue2;
4352 n=max(n,i);
4353 break;
4354 case 3:
4355 if(node->set_articulationParameterValue0 != old->set_articulationParameterValue3)
4356 node->articulationParameterArray.p[i] = node->set_articulationParameterValue3;
4357 n=max(n,i);
4358 break;
4359 case 4:
4360 if(node->set_articulationParameterValue0 != old->set_articulationParameterValue4)
4361 node->articulationParameterArray.p[i] = node->set_articulationParameterValue4;
4362 n=max(n,i);
4363 break;
4364 case 5:
4365 if(node->set_articulationParameterValue0 != old->set_articulationParameterValue5)
4366 node->articulationParameterArray.p[i] = node->set_articulationParameterValue5;
4367 n=max(n,i);
4368 break;
4369 case 6:
4370 if(node->set_articulationParameterValue0 != old->set_articulationParameterValue6)
4371 node->articulationParameterArray.p[i] = node->set_articulationParameterValue6;
4372 n=max(n,i);
4373 break;
4374 case 7:
4375 if(node->set_articulationParameterValue0 != old->set_articulationParameterValue7)
4376 node->articulationParameterArray.p[i] = node->set_articulationParameterValue7;
4377 n=max(n,i);
4378 break;
4379 default:
4380 break;
4381 }
4382 }
4383 node->articulationParameterCount = node->articulationParameterArray.n;
4384 }
4385 //printf("es %d inf %d for %d dr %d art %d\n ",node->_pduchange_es,es_info,es_force,es_deadreckoning,es_articulation);
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;
4389 }
4390 if(shallow_compare_node_fields(X3D_NODE(node),node->_oldState,FIELDS_events)){
4391 //node->_pduchange_collision = TRUE;
4392 //node->_pduchange_fire = TRUE;
4393 }
4394 if(shallow_compare_node_fields(X3D_NODE(node),node->_oldState,FIELDS_fire)){
4395 node->_pduchange_fire = TRUE;
4396 }
4397 if(shallow_compare_node_fields(X3D_NODE(node),node->_oldState,FIELDS_detonation)){
4398 node->_pduchange_detonation = TRUE;
4399 }
4400 if(shallow_compare_node_fields(X3D_NODE(node),node->_oldState,FIELDS_munition)){
4401 node->_pduchange_fire = TRUE;
4402 node->_pduchange_detonation = TRUE;
4403 }
4404 if(shallow_compare_node_fields(X3D_NODE(node),node->_oldState,FIELDS_rate)){
4405 node->_pduchange_fire = TRUE;
4406 node->_pduchange_detonation = TRUE;
4407 }
4408 }
4409 freeMallocedNodeFields(node->_oldState);
4410 shallow_copy_node(node->_oldState,X3D_NODE(node));
4411
4412}
4413
4414void compile_DISEntityManager0(struct X3D_DISEntityManager *node){
4415 //we use the same _pduchange flags and _oldState for both receiving and sending
4416 // but could be split if needed
4417 if(node->isNetworkReader){
4418 if(node->_pduchange_em_info){
4419 mark_changed_node_fields(X3D_NODE(node), node->_oldState, FIELDS_em_info);
4420 }
4421 if(node->_pduchange_create){
4422 if(node->addedEntities.n > 0)
4423 mark_changed_node_fields(X3D_NODE(node), node->_oldState, FIELDS_create);
4424 }
4425 if(node->_pduchange_remove){
4426 if(node->removedEntities.n > 0)
4427 mark_changed_node_fields(X3D_NODE(node), node->_oldState, FIELDS_remove);
4428 }
4429 reset_node_pduchanged(X3D_NODE(node));
4430
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;
4434 }
4435 if(shallow_compare_node_fields(X3D_NODE(node),node->_oldState,FIELDS_create)){
4436 if(node->addedEntities.n > 0)
4437 node->_pduchange_create = TRUE;
4438 }
4439 if(shallow_compare_node_fields(X3D_NODE(node),node->_oldState,FIELDS_remove)){
4440 if(node->removedEntities.n > 0)
4441 node->_pduchange_remove = TRUE;
4442 }
4443 }
4444 freeMallocedNodeFields(node->_oldState);
4445 shallow_copy_node(node->_oldState,X3D_NODE(node));
4446}
4447
4448
4449
4450
4451
4452// http://movesinstitute.org/~mcgredo/MV3500/hla/1278.1-200X%20Draft%2016%20rev%2018.pdf
4453// p.663 DR formula naming:
4454// rotation: fixed (F), rotating (R)
4455// order of position function: rate of position (P) (first order), rate of velocity (V) (second order)
4456// coords: world coordinates (W) body axis coordinates (B)
4457// p.665 dead reckoning formulas
4458enum {
4459STATIC = 1,
4460DRM_FPW = 2,
4461DRM_RPW = 3,
4462DRM_RVW = 4,
4463DRM_FVW = 5, //P = P0 + V0*dt + 1/2*A*dt^2 in world coords
4464DRM_FPB = 6,
4465DRM_RPB = 7,
4466DRM_RVB = 8,
4467DRM_FVB = 9, //P = P0 + (local2world)x(V0b*dt + 1/2*Ab*dt^2) convert to world after computing in local/entity/b=body space
4468};
4469void dead_reckon(int drmethod, double dtime, float *p1, float *R1xyza, float *p0, float *v0, float *a0, float *R0xyza, float *RVxyza){
4470 // freewrl: when we say world here, we mean TCS.
4471 // TCS == topocentric coordinate system, aka LGS Local Geodetic System, see Geospatial component, GeoLocation
4472 // LCS == local coordinate system - see Geospatial component, precision requirements, == TCS of geoOrigin / autoOrigin
4473 // DIS Local ~= web3d TCS, except with axes swizzled (DIS -Z up, X north, X3D Y up, -Z north)
4474 // we convert DR parameters in world system to/from web3d geo TCS system during pdu2node / node2pdu
4475 // so all below formula world coords are in TCS
4476 // any DR (dead reckoning) parameters in DIS-Local system are swizzled to/from web3d TCS convention in node2pdu and pdu2node
4477 // drmethod 1 - 9
4478 // dtime - time in seconds since last frame ie .01
4479 // p1 - output new location (TCS)
4480 // R1xyza - output new orientation (body2tcs)
4481 // p0 - location on last frame
4482 // v0 - linear velocity set on last send/recv
4483 // a0 - linear acceleration set on last send/recv
4484 // R0xyza - orientation on last frame Rbw
4485 // RVxyza - angular velocity, in entity/body
4486 switch(drmethod){
4487 //world coords
4488 case STATIC: //1
4489 veccopy3f(p1,p0);
4490 veccopy4f(R1xyza,R0xyza);
4491 break;
4492 case DRM_FPW: //2
4493 {
4494 float tmp[3];
4495 //update position
4496 // P = P0 + V0*dt
4497 vecadd3f(p1,p0,vecscale3f(tmp,v0,dtime));
4498 veccopy4f(R1xyza,R0xyza);
4499 }
4500 break;
4501 case DRM_RPW: //3
4502
4503 {
4504 float tmp[3];
4505 Quaternion qv, q1, q0;
4506 //update position
4507 // P = P0 + V0*dt
4508 vecadd3f(p1,p0,vecscale3f(tmp,v0,dtime));
4509 //update rotation
4510 // Rwb1 = DR(dt) * Rwb0
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);
4515
4516 }
4517 break;
4518 case DRM_RVW: //4
4519 {
4520 //update position
4521 //P = P0 + V0*dt + 1/2*A*dt^2 in world coords
4522 float tmp3[3],tmp2[3],tmp1[3];
4523 Quaternion qv, q1, q0;
4524
4525 vecadd3f(p1,p0,vecadd3f(tmp3,vecscale3f(tmp2,v0,dtime),vecscale3f(tmp1,a0,.5f*dtime*dtime)));
4526 //update rotation
4527 // Rwb1 = DR(dt) * Rwb0
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);
4532
4533 }
4534 break;
4535 case DRM_FVW: //5
4536
4537 {
4538 //F=fixed rotation, V = 2nd order, W=world coords
4539 //E.7.2.2 p.666
4540 //update position
4541 //P = P0 + V0*dt + 1/2*A*dt^2 in world coords
4542 float tmp3[3],tmp2[3],tmp1[3];
4543 vecadd3f(p1,p0,vecadd3f(tmp3,vecscale3f(tmp2,v0,dtime),vecscale3f(tmp1,a0,.5f*dtime*dtime)));
4544 //update rotation
4545 veccopy4f(R1xyza,R0xyza);
4546 }
4547 break;
4548
4549 //body/entity > A,V in body coords
4550 case DRM_FPB: //6
4551 {
4552 Quaternion qv, qa, q1, qbw;
4553 float deltap[3], att[3], tmp[3], tmp1[3], tmp2[3], tmp3[3];
4554
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); //world2body
4560 vecadd3f(p1,p0,tmp2);
4561
4562 //update rotation
4563 veccopy4f(R1xyza,R0xyza);
4564
4565 }
4566 break;
4567 case DRM_RPB: //7
4568 {
4569 Quaternion qv, qa, q1, qbw;
4570 float deltap[3], att[3], tmp[3], tmp1[3], tmp2[3], tmp3[3];
4571
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); //world2body
4577 vecadd3f(p1,p0,tmp2);
4578
4579 //update rotation
4580 // Rwb1 = DR(dt) * Rwb0
4581 quaternion_multiply(&q1,&qbw,&qv);
4582 quaternion_to_vrmlrot4f(&q1,R1xyza);
4583
4584 }
4585 break;
4586 case DRM_RVB: //8
4587 {
4588 //p.669
4589 //I think I see 2 problems with the formula they give:
4590 //1. their R1, R2 formula divide by |w|^n and when |w| is 0, that's divide by zero
4591 // - should produce Identity matrix when |w| is zero
4592 //2. P = P0 + Rbw*(R1*Vb + R2*Ab)
4593 // problem: when R1, R2 are Identity (when |w| 0), it doesn't look like V0*t + 1/2*A*t^2
4594 // should be:
4595 // P = P0 + Rbw*(R1*Vb*dt + R2*.5*Ab*dt*dt)
4596 // proposed simplification:
4597 // Rbb = Rv*dt (and maybe + Ra*.5*t^2) where bb means body pose update with dt
4598 // P = P0 + Rbw x Rbb(Vb*dt + Ab*.5*dt*dt)
4599 Quaternion qv, qa, q1, qbw;
4600 float deltap[3], att[3], tmp[3], tmp1[3], tmp2[3], tmp3[3];
4601
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); //world2body
4607 vecadd3f(p1,p0,tmp2);
4608
4609 //update rotation
4610 // Rwb1 = DR(dt) * Rwb0
4611 quaternion_multiply(&q1,&qbw,&qv);
4612 quaternion_to_vrmlrot4f(&q1,R1xyza);
4613
4614 }
4615 break;
4616 case DRM_FVB: //9
4617 {
4618 //P = P0 + (local2world)x(V0b*dt + 1/2*Ab*dt^2) convert to world after computing in local/entity/b=body space
4619 Quaternion qv, qa, q1, qbw;
4620 float deltap[3], att[3], tmp[3], tmp1[3], tmp2[3], tmp3[3];
4621
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); //world2body
4627 vecadd3f(p1,p0,tmp2);
4628
4629 //update rotation
4630 veccopy4f(R1xyza,R0xyza); //no update for 9
4631
4632 }
4633 break;
4634
4635 default:
4636 //update translation
4637 veccopy3f(p1,p0);
4638 //update rotation
4639 veccopy4f(R1xyza,R0xyza);
4640 break;
4641 }
4642
4643}
4644#define DRA_POS_THRSH 1.5
4645#define DRA_ORIENT_THRSH .175 //RADIANS about 10 degrees
4646
4647int transform_within_DeadReckoningTolerance1(struct X3D_EspduTransform *node){
4648 int withintol = TRUE;
4649 float p0[3], gap[3];
4650 struct X3D_EspduTransform *oldstate = (struct X3D_EspduTransform *)node->_oldState;
4651 veccopy3f(p0,node->_p0.c); //oldstate->translation.c);
4652
4653 vecdif3f(gap,p0,node->translation.c);
4654 if(veclength3f(gap) > DRA_POS_THRSH)
4655 withintol = FALSE;
4656 return withintol;
4657}
4658
4659void compile_EspduTransform1 (struct X3D_EspduTransform *node) {
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);
4664 }
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)) {
4668 //node->_pduchange_es = TRUE;
4669 if(!transform_within_DeadReckoningTolerance1(node)) {
4670 node->_pduchange_es = TRUE;
4671 }
4672 }
4673 }
4674 //whether its reader, writer or standalone, we need to compile if it changed state
4675 if(shallow_compare_node_fields(X3D_NODE(node),node->_oldState,FIELDS_es_transform)){
4676
4677 INITIALIZE_EXTENT;
4678
4679 /* printf ("changed Transform for node %u\n",node); */
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);
4685
4686 node->__do_anything = (node->__do_center ||
4687 node->__do_trans ||
4688 node->__do_scale ||
4689 node->__do_rotation ||
4690 node->__do_scaleO);
4691
4692 REINITIALIZE_SORTED_NODES_FIELD(node->children,node->_sortedChildren);
4693 MARK_NODE_COMPILED
4694 }
4695
4696}
4697void compile_EspduTransform (struct X3D_EspduTransform *node) {
4698 compile_DIS_common(node); // must be first in case need to initialize _oldState
4699 compile_EspduTransform1(node);
4700 compile_EspduTransform0(node); //must be last to re-copy node to oldstate
4701 MARK_NODE_COMPILED
4702}
4703
4704void espdu_update_by_dead_reckoning (struct X3D_EspduTransform *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];
4708 double dtime;
4709 static int smoothing_frames = 230; //frame count, at 60fps would be 4 seconds, ideally this would be a smoothing time in seconds
4710 static int want_smoothing = 1; //0;
4711
4712
4713 wasTransmitted = FALSE;
4714 if(node->isNetworkReader){
4715 if(node->_lasttime == 0.0)
4716 return;
4717 if(node->_pduchange_es){
4718 //node start or node received
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;
4726 }
4727 veccopy3f(node->_p0.c,node->translation.c);
4728 veccopy4f(node->_r0.c,node->rotation.c);
4729 }
4730 veccopy3f(p0,node->_p0.c);
4731 //veccopy3f(p0,node->translation.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);
4736
4737 }
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);
4744 }
4745 if(node->isNetworkWriter){
4746 if(node->_sent){
4747 //to be fair, only use what you send
4748 wasTransmitted = TRUE;
4749 node->_sent = FALSE;
4750 veccopy3f(node->_p0.c,node->translation.c);
4751 veccopy4f(node->_r0.c,node->rotation.c);
4752 }
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);
4758 }
4759 if(node->_lastframetime == 0.0)
4760 veccopy3f(node->_p0.c,p0);
4761 if(node->_lastframetime > 0.0){
4762 dtime = TickTime() - node->_lastframetime; //lastime();
4763 drmethod = node->deadReckoning;
4764 //if(drmethod)
4765 // if(!node->__geoSystem) drmethod = DRM_FVW; //if no geocoords, we'll assume transform is already in world coords
4766 dead_reckon(drmethod, dtime, p1, R1xyza, p0, v0, a0, R0xyza, RVxyza);
4767 veccopy3f(node->_p0.c,p1);
4768 veccopy4f(node->_r0.c,R1xyza);
4769 MARK_EVENT(X3D_NODE(node),offsetof(struct X3D_EspduTransform,_p0));
4770 }
4771 node->_lastframetime = TickTime();
4772 if(node->isNetworkReader){
4773 //update translation based on DR
4774 if(want_smoothing){
4775 //E.9 Smoothing p.678
4776 //just done on the receiver/isNetworkReader
4777 float psmooth[3], pzero[3], alpha;
4778 int n, i;
4779 node->_change++;
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++;
4787 }else{
4788 veccopy3f(node->translation.c,node->_p0.c);
4789 }
4790 veccopy4f(node->rotation.c,node->_r0.c);
4791 }
4792}
4793
4794/* do transforms, calculate the distance */
4795void prep_EspduTransform (struct X3D_EspduTransform *node) {
4796 if(node->isNetworkReader) espdu_update_by_dead_reckoning(node);
4797 COMPILE_IF_REQUIRED
4798 if(node->__geoSystem)
4799 geoprep(GEOSYS(node->__geoSystem),&node->geoCoords); //prep_EspduTransform0(node); //has render_vp filter
4800 if(!node->isNetworkReader) espdu_update_by_dead_reckoning(node);
4801 /* rendering the viewpoint means doing the inverse transformations in reverse order (while poping stack),
4802 * so we do nothing here in that case -ncoder */
4803
4804
4805 /* printf ("prep_Transform, render_hier vp %d geom %d light %d sens %d blend %d prox %d col %d\n",
4806 render_vp,render_geom,render_light,render_sensitive,render_blend,render_proximity,render_collision); */
4807
4808 /* do we have any geometry visible, and are we doing anything with geometry? */
4809 OCCLUSIONTEST
4810
4811 if(!renderstate()->render_vp) {
4812 /* do we actually have any thing to rotate/translate/scale?? */
4813 push_transform_local_identity();
4814
4815 if (node->__do_anything) {
4816
4817 FW_GL_PUSH_MATRIX();
4818 FW_GL_PUSH_MATRIX(); //this is to get us a separate 4x4 matrix just for the stuff here
4819 FW_GL_LOAD_IDENTITY(); // .. wehich we will save for child_Transform to propagate its bbox up to its extent
4820
4821 /* TRANSLATION */
4822 if (node->__do_trans)
4823 FW_GL_TRANSLATE_F(node->translation.c[0],node->translation.c[1],node->translation.c[2]);
4824
4825 /* CENTER */
4826 if (node->__do_center)
4827 FW_GL_TRANSLATE_F(node->center.c[0],node->center.c[1],node->center.c[2]);
4828
4829 /* ROTATION */
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]);
4832 }
4833
4834 /* SCALEORIENTATION */
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]);
4837 }
4838
4839
4840 /* SCALE */
4841 if (node->__do_scale)
4842 FW_GL_SCALE_F(node->scale.c[0],node->scale.c[1],node->scale.c[2]);
4843
4844 /* REVERSE SCALE ORIENTATION */
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]);
4847
4848 /* REVERSE CENTER */
4849 if (node->__do_center)
4850 FW_GL_TRANSLATE_F(-node->center.c[0],-node->center.c[1],-node->center.c[2]);
4851
4852 {
4853 double mat[16];
4854
4855 FW_GL_GETDOUBLEV(GL_MODELVIEW_MATRIX,mat); //we got our local transform saved
4856 FW_GL_POP_MATRIX();
4857 FW_GL_TRANSFORM_D(mat); //now apply the above to prep for child_Tranform
4858 reset_transform_local(mat);
4859 }
4860
4861 }
4862 }
4863
4864}
4865
4866
4867void fin_EspduTransform (struct X3D_EspduTransform *node) {
4868 OCCLUSIONTEST
4869
4870 if(!renderstate()->render_vp) {
4871 pop_transform_local();
4872 if (node->__do_anything) {
4873 FW_GL_POP_MATRIX();
4874 }
4875 } else {
4876 /*Rendering the viewpoint only means finding it, and calculating the reverse WorldView matrix.*/
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])
4879 );
4880 FW_GL_ROTATE_RADIANS(((node->scaleOrientation).c[3]),((node->scaleOrientation).c[0]),((node->scaleOrientation).c[1]),((node->scaleOrientation).c[2])
4881 );
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]))
4883 );
4884 FW_GL_ROTATE_RADIANS(-(((node->scaleOrientation).c[3])),((node->scaleOrientation).c[0]),((node->scaleOrientation).c[1]),((node->scaleOrientation).c[2])
4885 );
4886 FW_GL_ROTATE_RADIANS(-(((node->rotation).c[3])),((node->rotation).c[0]),((node->rotation).c[1]),((node->rotation).c[2])
4887 );
4888 FW_GL_TRANSLATE_F(-(((node->center).c[0])),-(((node->center).c[1])),-(((node->center).c[2]))
4889 );
4890 FW_GL_TRANSLATE_F(-(((node->translation).c[0])),-(((node->translation).c[1])),-(((node->translation).c[2]))
4891 );
4892 }
4893 }
4894 if(node->__geoSystem)
4895 geofin(GEOSYS(node->__geoSystem),&node->geoCoords); //has vp_render filters //fin_EspduTransform0(node);
4896
4897}
4898void render_munitions(struct X3D_EspduTransform *node){
4899 //I have no ideas. something about quantity, velocity, start/end or startpoint
4900 //a) update locations based on time and trajectory - like partical physics
4901 //b) render each munition instance
4902 if(!renderstate()->render_vp) {
4903
4904 if(node->fired1){
4905 int i;
4906 struct X3D_EspduTransform * mnode;
4907 static int eventNumber = 0;
4908 if(node->eventNumber > eventNumber){
4909 node->firedTime = TickTime();
4910 eventNumber = node->eventNumber;
4911 }
4912 double dtime = TickTime() - (double)node->munitionQuantity/(double)max(1,node->firingRate) - node->firedTime ;
4913 if(dtime > 5.0) return; //already finished
4914 mnode = (struct X3D_EspduTransform*)dis_find_registered_node_by_entityid(node->munitionEntityID,TRUE,TRUE);
4915 if(mnode){
4916 for(i=0;i<node->munitionQuantity;i++){
4917 //how about a 1 second gap between burst pals
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)) {
4925 // detonate or whatever you do when munition reaches target
4926 veccopy3f(loc,node->munitionEndPoint.c);
4927 if(i==(node->munitionQuantity-1)){
4928 node->fired1 = FALSE; //last munition in burst hit target
4929 node->detonateTime = TickTime();
4930 }
4931 } else {
4932 vecadd3f(loc,node->munitionStartPoint.c,progress);
4933 // render munition instance
4934 }
4935 FW_GL_PUSH_MATRIX();
4936 FW_GL_TRANSLATE_F(loc[0],loc[1],loc[2]);
4937 //static int k = 0;
4938 //if(k++ % 120 == 0)
4939 // printf("%lf %lf %lf\n",loc[0],loc[1],loc[2]);
4940 //strip espdu wrapper (otherwise we have geoLocation wrapping geoLocation - double geo transform
4941 normalChildren(mnode->children);
4942 FW_GL_POP_MATRIX();
4943 }
4944 }
4945 }
4946 }
4947}
4948void render_detonation(struct X3D_EspduTransform *node){
4949 //I have no ideas. something about quantity, velocity, start/end or startpoint
4950 //a) update locations based on time and trajectory - like partical physics
4951 //b) render each munition instance
4952 if(!renderstate()->render_vp) {
4953 double dtime = TickTime() - node->detonateTime;
4954 if( dtime > 0.0 && dtime < .5){
4955 int i;
4956 struct X3D_EspduTransform * mnode;
4957 mnode = (struct X3D_EspduTransform*)dis_find_registered_node_by_entityid(node->munitionEntityID,TRUE,TRUE);
4958 if(mnode){
4959 for(i=0;i<node->munitionQuantity;i++){
4960 float loc[3], fscale;
4961 //how about a 1 second gap between burst pals
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);
4968 FW_GL_POP_MATRIX();
4969 }
4970 }
4971 }
4972 }
4973}
4974void dis_register_collide(struct X3D_Node* node,double *transform);
4975void child_EspduTransform (struct X3D_EspduTransform *node) {
4976 CHILDREN_COUNT
4977 OCCLUSIONTEST
4978
4979 RETURN_FROM_CHILD_IF_NOT_FOR_ME
4980
4981 /* any children at all? */
4982 if (nc==0) return;
4983 {
4984 double modelviewMatrix[16];
4985 FW_GL_GETDOUBLEV(GL_MODELVIEW_MATRIX, modelviewMatrix);
4986 dis_register_collide(X3D_NODE(node),modelviewMatrix);
4987 }
4988 prep_sibAffectors((struct X3D_Node*)node,&node->__sibAffectors);
4989
4990 prep_BBox((struct BBoxFields*)&node->bboxCenter);
4991
4992 normalChildren(node->_sortedChildren);
4993 //render munitions
4994 render_munitions(node);
4995 //render detonations
4996 render_detonation(node);
4997 //render collisions
4998
4999 fin_BBox((struct X3D_Node*)node,(struct BBoxFields*)&node->bboxCenter,TRUE);
5000
5001 fin_sibAffectors((struct X3D_Node*)node,&node->__sibAffectors);
5002}
5003
5004
5005// >> RADIO
5006// first parts of radio node structs ynchronized so as to match Espdu struct so
5007// the 3 radio nodes can be cast to EspduTransform for common field handling
5008void compile_TransmitterPdu (struct X3D_TransmitterPdu *node) {
5009 compile_DIS_common((struct X3D_EspduTransform *)node); //assumes transform padding in transmitter node
5010 compile_TransmitterPdu0(node);
5011 MARK_NODE_COMPILED
5012}
5013void compile_SignalPdu (struct X3D_SignalPdu *node) {
5014 compile_DIS_common((struct X3D_EspduTransform *)node); //assumes transform padding in signal node
5015 compile_SignalPdu0(node);
5016 MARK_NODE_COMPILED
5017}
5018void compile_ReceiverPdu (struct X3D_ReceiverPdu *node) {
5019 compile_DIS_common((struct X3D_EspduTransform *)node); //assumes transform padding in receiver node
5020 compile_ReceiverPdu0(node);
5021 MARK_NODE_COMPILED
5022}
5023
5024void child_TransmitterPdu (struct X3D_TransmitterPdu *node) {
5025 COMPILE_IF_REQUIRED
5026 geoprep(GEOSYS(node->__geoSystem),&node->geoCoords);
5027 //do stuff
5028 geofin(GEOSYS(node->__geoSystem),&node->geoCoords);
5029}
5030void child_SignalPdu (struct X3D_SignalPdu *node) {
5031 COMPILE_IF_REQUIRED
5032 geoprep(GEOSYS(node->__geoSystem),&node->geoCoords);
5033 //do stuff
5034 geofin(GEOSYS(node->__geoSystem),&node->geoCoords);
5035}
5036void child_ReceiverPdu (struct X3D_ReceiverPdu *node) {
5037 COMPILE_IF_REQUIRED
5038 geoprep(GEOSYS(node->__geoSystem),&node->geoCoords);
5039 //do stuff
5040 geofin(GEOSYS(node->__geoSystem),&node->geoCoords);
5041}
5042
5043//<< RADIO
5044
5045void print_entitymapping(struct X3D_DISEntityTypeMapping *anode){
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);
5048}
5049void compile_DISEntityManager(struct X3D_DISEntityManager *node){
5050 compile_DIS_network((struct X3D_EspduTransform *)node);
5051 compile_DISEntityManager0(node);
5052 MARK_NODE_COMPILED
5053}
5054static int app_entity_last_id = 0;
5055int newEntityID(){
5056//for current app instance
5057 app_entity_last_id++;
5058 return app_entity_last_id;
5059}
5060#define GEOEL_WE_A (double)6378137
5061void child_DISEntityManager(struct X3D_DISEntityManager *node){
5062 //Problem: web3d doesn't have a sender entitymanager. So its dependant on other (unknown) ?commercial? programs.
5063 //Solution 1: modify DISEntityManager to have networkMode='networkWriter'
5064 // and an MFnode initializeOnly field of EntityTypeMapping nodes
5065 //Solution 2: 'entity discovery' by listening -> entity manager for creation
5066 // - done in the pdu receive loop, if there are 'leftover pdus' they are
5067 // examined as candidates for entity discovery, and sent here via .addEntities
5068 static int ADD = 1, REMOVE = 2;
5069 COMPILE_IF_REQUIRED
5070 //like add remove children in opengl utils
5071 if(node->addEntities.n){
5072 int i,j;
5073 struct Multi_Node* mfn = &node->entities;
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;
5080 struct X3D_Node *candi;
5081 struct X3D_DISEntityTypeMapping *best;
5082 int use_GC = FALSE;
5083
5084 // = (struct X3D_DISEntityTypeMapping *)node->addEntities.p[j];
5085 ibest = -1;
5086 iscore = 0;
5087 best = NULL;
5088 candi = node->addEntities.p[j];
5089 if(candi->_nodeType == NODE_DISEntityTypeMapping)
5090 {
5091 //problem: the DISEntityTypeMapping doesn't have a field for entityID
5092 // - that's OK when we are just told to create-and-own a new entity
5093 // -we can assign our siteID, applicationID and increment our entityID count for entityID
5094 // x but not for entity discovery
5095 // * so we added some fields _entityID,_applicationID,_siteID for copying from sniffed pdu
5096 // and sending to the .addEntities list
5097 //print_entitymapping(anode);
5098 struct X3D_DISEntityTypeMapping *anode = (struct X3D_DISEntityTypeMapping *)node->addEntities.p[j];
5099 struct Multi_Node* mapping = &node->mapping;
5100 if (mapping->n == 0) mapping = &node->children;
5101 for(i=0;i<mapping->n;i++){
5102 struct X3D_DISEntityTypeMapping *bnode = (struct X3D_DISEntityTypeMapping *)mapping->p[i];
5103 //printf("compare %d",i);
5104 //print_entitymapping(bnode);
5105 jscore = 0;
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){
5114 iscore = jscore;
5115 ibest = i;
5116 best = bnode;
5117 }
5118 }
5119 printf("\niscore %d ibest %d best.url %s\n", iscore, ibest, best->url.p[0]->strptr);
5120 if(ibest > -1){
5121 applicationID = node->applicationID;
5122 siteID = node->siteID;
5123 entityID = newEntityID(); //anode->_entityID;
5124 address = node->address;
5125 port = node->port;
5126 networkMode = newASCIIString ("networkWriter"); //if we're ordered to create, usually that also means own
5127 multicastRelayHost = node->multicastRelayHost;
5128 multicastRelayPort = node->multicastRelayPort;
5129
5130 }
5131 } else if(candi->_nodeType == NODE_EspduTransform) {
5132 // || candi->_nodeType == NODE_ReceiverPdu
5133 // || candi->_nodeType == NODE_TransmitterPdu || candi->_nodeType == NODE_SignalPdu){
5134 //this comes from 'entity discovery' from leftovver pdus
5135 struct X3D_EspduTransform *anode = (struct X3D_EspduTransform *)node->addEntities.p[j];
5136 for(i=0;i<node->mapping.n;i++){
5137 struct X3D_DISEntityTypeMapping *bnode = (struct X3D_DISEntityTypeMapping *)node->mapping.p[i];
5138 //printf("compare %d",i);
5139 //print_entitymapping(bnode);
5140 jscore = 0;
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){
5149 iscore = jscore;
5150 ibest = i;
5151 best = bnode;
5152 }
5153 }
5154 //printf("etm %d %d %d %d %d %d %d\n", best->kind, best->domain, best->country, best->category,
5155 // best->subcategory, best->specific, best->extra);
5156 //printf("es %d %d %d %d %d %d %d\n", anode->entityKind, anode->entityDomain, anode->entityCountry,
5157 // anode->entityCategory, anode->entitySubCategory, anode->entitySpecific, anode->entityExtra);
5158 //printf("\niscore %d ibest %d best.url %s\n", iscore, ibest, best->url.p[0]->strptr);
5159 if(ibest > -1){
5160 applicationID = anode->applicationID;
5161 siteID = anode->siteID;
5162 entityID = anode->entityID;
5163 address = anode->address;
5164 port = anode->port;
5165 networkMode = newASCIIString ("networkReader"); //if we discovered entity by its heartbeats, then we're reading
5166 multicastRelayHost = anode->multicastRelayHost;
5167 multicastRelayPort = anode->multicastRelayPort;
5168 if(veclengthd(anode->geoCoords.c) < GEOEL_WE_A/2.0) use_GC = TRUE; //earths core GD,WE doesn't work well here
5169 }
5170 }
5171
5172 if(ibest > -1){
5173 int isgroup = 0;
5174 //printf("ibest = %d iscore= %d url=%s\n",ibest,iscore,best->url.p[0]->strptr);
5175 //if (best->_child == NULL) {
5176 struct X3D_Inline * iline;
5177 struct X3D_EspduTransform *espdu;
5178 //struct X3D_Group *grp;
5179 iline = createNewX3DNode(NODE_Inline); //this assigns a parent resource using parsing thread methods, which is wrong for rendering thread
5180 //resource_item_t *pres = iline->_parentResource;
5181 iline->_parentResource = X3D_PROTO(node->_executionContext)->_parentResource; //for rendering-thread creation of inlines, use the parent context's parentResource
5182 //if(isgroup){
5183 // grp = createNewX3DNode(NODE_Group);
5184 //}else{
5185 //this is 'normal' according to specs we are supposed to generate espdus
5186 espdu = createNewX3DNode(NODE_EspduTransform);
5187 if(use_GC) {
5188 espdu->geoSystem.p[0] = newASCIIString("GC");
5189 espdu->geoSystem.n = 1;
5190 }
5191 //populate entity fields - so it starts swallowing the heartbeat and update pdus of the entity
5192 espdu->enabled = TRUE;
5193 espdu->isActive = TRUE;
5194 espdu->entityID = entityID;
5195 espdu->applicationID = applicationID;
5196 espdu->siteID = siteID;
5197 espdu->port = port;
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());
5203
5204 /*
5205 void *dis_register(struct X3D_Node* node,char *address,int applicationID,int entityID,char *multicastRelayHost,
5206 int multicastRelayPort,
5207 char *networkMode, int port,double readInterval,int rtpHeaderExpected,int siteID,double writeInterval)
5208 */
5209 dis_register(X3D_NODE(espdu),address->strptr,applicationID,entityID,multicastRelayHost->strptr,multicastRelayPort,
5210 networkMode->strptr,port,5.0,FALSE,siteID,5.0);
5211 //}
5212 //if(best->_executionContext){
5213 add_node_to_broto_context(X3D_PROTO(node->_executionContext),X3D_NODE(iline));
5214 //if(isgroup)
5215 // add_node_to_broto_context(X3D_PROTO(node->_executionContext),X3D_NODE(grp));
5216 //else
5217 add_node_to_broto_context(X3D_PROTO(node->_executionContext),X3D_NODE(espdu));
5218 //}
5219 //best->_child = isgroup ? X3D_NODE(grp) : X3D_NODE(espdu);
5220
5221 //ADD_PARENT(X3D_NODE(best->_child), X3D_NODE(best));
5222 //if(isgroup)
5223 // AddRemoveChildren(X3D_NODE(grp), &grp->children, (struct X3D_Node * *)&iline, 1, ADD,__FILE__,__LINE__);
5224 //else
5225 AddRemoveChildren(X3D_NODE(espdu), &espdu->children, (struct X3D_Node * *)&iline, 1, ADD,__FILE__,__LINE__);
5226 /* copy over the URL from parent */
5227 shallow_copy_field(FIELDTYPE_MFString,(union anyVrml*)&best->url,(union anyVrml*)&iline->url);
5228 iline->load = TRUE;
5229 //}
5230
5231 AddRemoveChildren(X3D_NODE(node), mfn, (struct X3D_Node * *)&espdu, 1, ADD,__FILE__,__LINE__);
5232 //AddRemoveChildren(X3D_NODE(node), &node->addedEntities, (struct X3D_Node * *)&best->_child, 1, ADD,__FILE__,__LINE__);
5233 AddRemoveChildren(X3D_NODE(node), &node->addedEntities, (struct X3D_Node * *)&espdu, 1, ADD,__FILE__,__LINE__);
5234
5235 }
5236
5237 }
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);
5241 }
5242 if(node->removeEntities.n){
5243 int i,j;
5244 struct Multi_Node* mfn = &node->entities;
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;
5249 struct X3D_DISEntityTypeMapping *best, *anode = (struct X3D_DISEntityTypeMapping *)node->removeEntities.p[j];
5250 ibest = -1;
5251 iscore = 0;
5252 best = NULL;
5253 for(i=0;i<node->mapping.n;i++){
5254 if(node->mapping.p[i]->_nodeType == NODE_DISEntityTypeMapping){
5255 struct X3D_DISEntityTypeMapping *bnode = (struct X3D_DISEntityTypeMapping *)node->mapping.p[i];
5256 jscore = 0;
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){
5265 iscore = jscore;
5266 ibest = i;
5267 best = bnode;
5268 }
5269 }
5270 }
5271 if(ibest > -1){
5272 //printf("remove: ibest = %d iscore= %d url=%s\n",ibest,iscore,best->url.p[0]->strptr);
5273 AddRemoveChildren(X3D_NODE(node), mfn, (struct X3D_Node * *)&best, 1, REMOVE,__FILE__,__LINE__);
5274 if(best->_child)
5275 AddRemoveChildren(X3D_NODE(node), &node->removedEntities, (struct X3D_Node * *)&best->_child, 1, ADD,__FILE__,__LINE__);
5276 }
5277 //else
5278 // printf("remove: no match found\n");
5279 }
5280 }
5281 if(node->removedEntities.n) {
5282 //printf("removedEntities.n=%d\n",node->removedEntities.n);
5283 MARK_EVENT(X3D_NODE(node),offsetof(struct X3D_DISEntityManager,removedEntities));
5284 }
5285 node->removeEntities.n = 0;
5286 }
5287}
5288Stack *dis_collide_stack = NULL;
5289void dis_collide(){
5290 int i,j;
5291 if(dis_collide_stack){
5292 for(i=0;i<dis_collide_stack->n;i++){
5293 int ihit;
5294 float ee[6];
5295 double mvmInverse[16], m2m[16];
5296 struct X3D_EspduTransform *espdu;
5297
5298 usehit *uhit = vector_get_ptr(usehit,dis_collide_stack,i);
5299 espdu = (struct X3D_EspduTransform*)uhit->node;
5300 if(espdu->isNetworkReader) continue; //do only OWNED/isWriter,isNeutral, listen for the rest
5301 ihit = 0;
5302 //invert matrix
5303 matinverseAFFINE(mvmInverse,uhit->mvm);
5304 extent6f_copy(ee,uhit->node->_extent);
5305 for(j=0;j<dis_collide_stack->n;j++){
5306 if(j != i){
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)){
5311 //multiply matrices
5312 matmultiplyAFFINE(m2m,mvmInverse,uhitb->mvm);
5313 //convert B extent to A-space
5314 extent6f_mattransform4d(eeba,eeb,m2m);
5315 //compare extents
5316 extent6f_intersect_extent6f(eaXb,ee,eeba);
5317 if(extent6f_isSet(eaXb)){
5318 //they overlap/intersect/collide
5319 //extent6f_printf(ee); printf("ee \n");
5320 //extent6f_printf(eeb); printf("eeb \n");
5321 //extent6f_printf(eeba); printf("eeba\n");
5322 //extent6f_printf(eaXb); printf("eaXb\n");
5323 //we'll just change A, and just for its collistion with B
5324 struct X3D_EspduTransform *espdub = (struct X3D_EspduTransform*)uhitb->node;
5325 if(espdu->isCollided == FALSE){
5326 espdu->collideTime = TickTime();
5327 espdu->eventNumber = dis_next_event_number();
5328 }
5329 espdu->collisionType = 33;
5330 espdu->isCollided = TRUE;
5331 espdu->eventSiteID = espdub->siteID;
5332 espdu->eventApplicationID = espdub->applicationID;
5333 espdu->eventEntityID = espdub->entityID;
5334 ihit++;
5335 //H we automatically do this during node compile:
5336 MARK_EVENT(X3D_NODE(espdu),offsetof(struct X3D_EspduTransform,isCollided));
5337 break;
5338 }
5339 }
5340 }
5341 }
5342 if(ihit == 0) {
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;
5350 MARK_EVENT(X3D_NODE(espdu),offsetof(struct X3D_EspduTransform,isCollided));
5351 }
5352 }
5353 }
5354 }
5355}
5356void dis_clear_collide(){
5357 if(dis_collide_stack) dis_collide_stack->n = 0;
5358}
5359void dis_register_collide(struct X3D_Node* node,double *transform){
5360 //call from child_espduTransform
5361 int i, ifound;
5362 if(!dis_collide_stack) dis_collide_stack = newStack(usehit);
5363 ifound = -1;
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){
5367 ifound = i;
5368 break;
5369 }
5370 }
5371 if(ifound == -1){
5372 usehit uhit;
5373 uhit.node = node;
5374 memcpy(uhit.mvm,transform,16*sizeof(double));
5375 uhit.userdata = NULL;
5376 vector_pushBack(usehit,dis_collide_stack,uhit);
5377 }
5378
5379}
5380void dis_initialize() {
5381 //this is for the 2023 experimental multiplayer sensor synchronization and avatar update methods
5382 //it relies on freewrl commandline parameter values for DIS, rather than in-scene DIS node field values
5383 static int once = 0;
5384 if (!once) {
5385 // load default dis_socket
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();
5390
5391 struct dis_socket dsock, * psock;
5392 //create a recv socket
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("");// multicastRelayHost;
5400 psock->multicastRelayPort = 0; // multicastRelayPort;
5401 psock->idir = 1;
5402 //open port
5403 dis_open_socket(psock);
5404
5405 //create a send socket
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("");// multicastRelayHost;
5413 psock->multicastRelayPort = 0;// multicastRelayPort;
5414 psock->idir = 2;
5415 //open port
5416 dis_open_socket(psock);
5417 once = 1;
5418 }
5419}
5420#else //WITH_DIS
5421
5422void compile_DISEntityManager(struct X3D_DISEntityManager *node){}
5423void child_DISEntityManager(struct X3D_DISEntityManager *node){}
5424void compile_TransmitterPdu(struct X3D_TransmitterPdu *node){}
5425void child_TransmitterPdu(struct X3D_TransmitterPdu *node){}
5426void compile_SignalPdu(struct X3D_SignalPdu *node){}
5427void child_SignalPdu(struct X3D_SignalPdu *node){}
5428void compile_ReceiverPdu(struct X3D_ReceiverPdu *node){}
5429void child_ReceiverPdu(struct X3D_ReceiverPdu *node){}
5430void compile_EspduTransform (struct X3D_EspduTransform *node) {}
5431void prep_EspduTransform(struct X3D_EspduTransform *node){}
5432void fin_EspduTransform(struct X3D_EspduTransform *node){}
5433void child_EspduTransform(struct X3D_EspduTransform *node){}
5434void dis_initialize() {}
5435#endif //WITH_DIS
5436
5437void fwl_sendreceive_DIS(){
5438 //just the buffer in/out is handled here
5439 //the interpretation/parsing/packing of pdus is done in the backend
5440 //this might need to be in the front end so platforms with sandbox restrictions on communication
5441 //can do this in the native language/technology if necessary - I'll find out later.
5442/* pseudo code design:
5443 if(dis_sendlist.n > 0){
5444 //backend will queue up pdus to send,
5445 //frontend fetches them one by one from the queue
5446 //this isolates potentialy platform-specific things like networking in frontend
5447 //while avoiding having the backend call into the frontend which is often in a dfferent
5448 //language technology
5449 loop over sendlist:
5450 (n,buf,ip,port) = get_next_send_from_backend()
5451 sendTo(buf,n,ip,port)
5452 // flushing queue should be done in BACKEND, start of each loop
5453 // so if no frontend capability, the queue doesn't overflow
5454 }
5455 if(dis_recvlist.n > 0){
5456 loop over all recv channels or connect-switch or libevent
5457 if(not yet opened)
5458 open
5459 non-blocking recv or recvfrom or recv with short timeout
5460 if(got something) dis_incoming_to_backend(ip,port,stream,len)
5461 }
5462*/
5463 if(allow_DIS){
5464#ifdef WITH_DIS
5465 //printf("yo from fwl_sendreceive_DIS\n");
5466 dis_initialize();
5467 dis_collide();
5468 dis_sendloop();
5469 dis_recvloop();
5470 dis_clear_collide();
5471#endif //WITH_DIS
5472 }
5473}
5474//
5475//#ifdef WITH_DIS
5476//#include "../DIS/DIS.c"
5477//#endif //WITH_DIS
unsigned short quantity
how many of the munition were fired
Definition DIS.h:840
unsigned short rate
rate at which the munition was fired
Definition DIS.h:842
struct EntityType munition
What munition was used in the burst.
Definition DIS.h:834
unsigned short warhead
type of warhead
Definition DIS.h:836
unsigned short fuse
type of fuse used
Definition DIS.h:838
unsigned char collisionType
ID of event.
Definition DIS.h:1807
struct EventID eventID
ID of event.
Definition DIS.h:1805
struct EntityID issuingEntityID
ID of the entity that issued the collision PDU.
Definition DIS.h:1801
struct EntityID collidingEntityID
ID of entity that has collided with the issuing entity ID.
Definition DIS.h:1803
void * variableDatums
variable length list of variable length datums
Definition DIS.h:1719
unsigned int numberOfVariableDatumRecords
Number of variable datum records.
Definition DIS.h:1715
struct Vector3Float entityAngularVelocity
angular velocity of the entity
Definition DIS.h:742
struct Vector3Float entityLinearAcceleration
Linear acceleration of the entity.
Definition DIS.h:740
unsigned char deadReckoningAlgorithm
enumeration of what dead reckoning algorighm to use
Definition DIS.h:736
char otherParameters[15]
other parameters to use in the dead reckoning algorithm
Definition DIS.h:738
struct Vector3Float locationInEntityCoordinates
location of the detonation or impact in the target entity's coordinate system.
Definition DIS.h:1749
struct BurstDescriptor burstDescriptor
Describes munition used.
Definition DIS.h:1747
struct Vector3Double locationInWorldCoordinates
where the detonation is, in world coordinates
Definition DIS.h:1745
unsigned char detonationResult
result of the explosion
Definition DIS.h:1751
struct EventID eventID
ID firing event.
Definition DIS.h:1741
unsigned char numberOfArticulationParameters
How many articulation parameters we have.
Definition DIS.h:1753
struct EntityID munitionID
ID of muntion that was fired.
Definition DIS.h:1739
struct Vector3Float velocity
ID firing event.
Definition DIS.h:1743
unsigned short application
The application ID.
Definition DIS.h:575
unsigned short site
The site ID.
Definition DIS.h:573
unsigned short entity
the entity ID
Definition DIS.h:577
struct Vector3Float entityLinearVelocity
Describes the speed of the entity in the world.
Definition DIS.h:2145
struct EntityType entityType
Describes the type of entity in the world.
Definition DIS.h:2142
struct EntityID entityID
Unique ID for an entity that is tied to this state information.
Definition DIS.h:2136
void * articulationParameters
variable length list of articulation parameters
Definition DIS.h:2159
struct Orientation entityOrientation
describes the orientation of the entity, in euler angles
Definition DIS.h:2149
int entityAppearance
a series of bit flags that are used to help draw the entity, such as smoking, on fire,...
Definition DIS.h:2151
struct Vector3Double entityLocation
describes the location of the entity in the world
Definition DIS.h:2147
struct DeadReckoningParameter deadReckoningParameters
parameters used for dead reckoning
Definition DIS.h:2153
char numberOfArticulationParameters
How many articulation parameters are in the variable length list.
Definition DIS.h:2140
unsigned char domain
Domain of entity (air, surface, subsurface, space, etc)
Definition DIS.h:482
unsigned short country
country to which the design of the entity is attributed
Definition DIS.h:484
unsigned char entityKind
Kind of entity.
Definition DIS.h:480
unsigned char specific
specific info based on subcategory field
Definition DIS.h:490
unsigned char category
category of entity
Definition DIS.h:486
unsigned char subcategory
subcategory of entity
Definition DIS.h:488
unsigned short application
The application ID.
Definition DIS.h:226
unsigned short eventNumber
the number of the event
Definition DIS.h:228
unsigned short site
The site ID.
Definition DIS.h:224
struct BurstDescriptor burstDescriptor
Describes munitions used in the firing event.
Definition DIS.h:1846
struct EntityID munitionID
ID of the munition that is being shot.
Definition DIS.h:1839
struct Vector3Float velocity
Velocity of the ammunition.
Definition DIS.h:1848
struct EventID eventID
ID of event.
Definition DIS.h:1841
struct Vector3Double locationInWorldCoordinates
location of the firing event
Definition DIS.h:1844
float range
range to the target
Definition DIS.h:1850
unsigned short system
system
Definition DIS.h:346
unsigned short detail
detail
Definition DIS.h:344
unsigned short spreadSpectrum
spread spectrum, 16 bit boolean array
Definition DIS.h:340
unsigned short major
major
Definition DIS.h:342
Definition DIS.h:597
unsigned char pduType
Type of pdu, unique for each PDU class.
Definition DIS.h:603
short padding
zero-filled array of padding
Definition DIS.h:611
unsigned short radioId
particular radio within an entity
Definition DIS.h:881
struct EntityID entityId
ID of the entitythat is the source of the communication.
Definition DIS.h:879
unsigned short country
country to which the design of the entity is attributed
Definition DIS.h:174
unsigned char domain
Domain of entity (air, surface, subsurface, space, etc)
Definition DIS.h:172
unsigned char entityKind
Kind of entity.
Definition DIS.h:170
unsigned char category
category of entity
Definition DIS.h:176
unsigned char nomenclatureVersion
specific info based on subcategory field
Definition DIS.h:178
struct EntityID transmitterEntityId
ID of transmitter.
Definition DIS.h:1863
unsigned short transmitterRadioId
ID of transmitting radio.
Definition DIS.h:1865
unsigned short receiverState
encoding scheme used, and enumeration
Definition DIS.h:1857
float receivedPoser
received power
Definition DIS.h:1861
void * data
list of eight bit values
Definition DIS.h:1939
unsigned int sampleRate
sample rate
Definition DIS.h:1933
unsigned short encodingScheme
encoding scheme used, and enumeration
Definition DIS.h:1929
short dataLength
length od data
Definition DIS.h:1935
unsigned short tdlType
tdl type
Definition DIS.h:1931
short samples
number of samples
Definition DIS.h:1937
struct EntityID originatingEntityID
Entity that is sending message.
Definition DIS.h:801
unsigned char modulationParameterCount
how many modulation parameters we have
Definition DIS.h:1261
unsigned short antennaPatternType
antenna pattern type
Definition DIS.h:1245
unsigned char transmitState
transmit state
Definition DIS.h:1235
float power
transmission power
Definition DIS.h:1253
unsigned short cryptoSystem
crypto system enumeration
Definition DIS.h:1257
float transmitFrequencyBandwidth
transmit frequency Bandwidth
Definition DIS.h:1251
struct RadioEntityType radioEntityType
linear accelleration of entity
Definition DIS.h:1233
struct Vector3Double antennaLocation
Location of antenna.
Definition DIS.h:1241
struct Vector3Float relativeAntennaLocation
relative location of antenna
Definition DIS.h:1243
unsigned short cryptoKeyId
crypto system key identifer
Definition DIS.h:1259
unsigned char inputSource
input source
Definition DIS.h:1237
unsigned short antennaPatternCount
atenna pattern length
Definition DIS.h:1247
unsigned long long frequency
frequency
Definition DIS.h:1249
struct ModulationType modulationType
modulation
Definition DIS.h:1255
unsigned int variableDatumID
ID of the variable datum.
Definition DIS.h:617
void * variableDatums
variable length list of 64-bit datums
Definition DIS.h:621
unsigned int variableDatumLength
length of the variable datums
Definition DIS.h:619
double x
X value.
Definition DIS.h:589
double y
Y value.
Definition DIS.h:591
double z
Z value.
Definition DIS.h:593
float z
Z value.
Definition DIS.h:334
float y
y Value
Definition DIS.h:332
float x
X value.
Definition DIS.h:330
struct EntityID firingEntityID
ID of the entity that shot.
Definition DIS.h:997