dis_rs/common/entity_state/
parser.rs

1use crate::common::entity_state::model::{
2    DrEulerAngles, DrOtherParameters, DrParameters, DrWorldOrientationQuaternion, EntityAppearance,
3    EntityMarking, EntityState,
4};
5use crate::common::model::{EntityType, PduBody, PduHeader};
6use crate::common::parser;
7use crate::common::parser::{entity_id, entity_type, sanitize_marking, vec3_f32};
8use crate::enumerations::{
9    DeadReckoningAlgorithm, EntityMarkingCharacterSet, ForceId, ProtocolVersion,
10};
11use crate::v6::entity_state::parser::entity_capabilities;
12use nom::bytes::complete::take;
13use nom::multi::count;
14use nom::number::complete::{be_f32, be_u16, be_u32, be_u8};
15use nom::IResult;
16
17pub(crate) fn entity_state_body(
18    header: &PduHeader,
19) -> impl Fn(&[u8]) -> IResult<&[u8], PduBody> + '_ {
20    move |input: &[u8]| {
21        let (input, entity_id_val) = entity_id(input)?;
22        let (input, force_id_val) = force_id(input)?;
23        let (input, variable_parameters_no) = be_u8(input)?;
24        let (input, entity_type_val) = entity_type(input)?;
25        let (input, alternative_entity_type) = entity_type(input)?;
26        let (input, entity_linear_velocity) = vec3_f32(input)?;
27        let (input, entity_location) = parser::location(input)?;
28        let (input, entity_orientation) = parser::orientation(input)?;
29        let (input, entity_appearance) = entity_appearance(entity_type_val)(input)?;
30        let (input, dead_reckoning_parameters) = dr_parameters(input)?;
31        let (input, entity_marking) = entity_marking(input)?;
32        #[allow(clippy::wildcard_in_or_patterns)]
33        let (input, entity_capabilities) =
34            if header.protocol_version == ProtocolVersion::IEEE1278_12012 {
35                crate::v7::entity_state::parser::entity_capabilities(entity_type_val)(input)?
36            } else {
37                let (input, entity_capabilities) = entity_capabilities(input)?;
38                (
39                    input,
40                    crate::enumerations::EntityCapabilities::from(entity_capabilities),
41                )
42            };
43        let (input, variable_parameters) = if variable_parameters_no > 0 {
44            count(parser::variable_parameter, variable_parameters_no as usize)(input)?
45        } else {
46            (input, vec![])
47        };
48
49        let body = EntityState::builder()
50            .with_entity_id(entity_id_val)
51            .with_force_id(force_id_val)
52            .with_entity_type(entity_type_val)
53            .with_alternative_entity_type(alternative_entity_type)
54            .with_velocity(entity_linear_velocity)
55            .with_location(entity_location)
56            .with_orientation(entity_orientation)
57            .with_appearance(entity_appearance)
58            .with_dead_reckoning_parameters(dead_reckoning_parameters)
59            .with_marking(entity_marking)
60            .with_capabilities(entity_capabilities)
61            .with_variable_parameters(variable_parameters)
62            .build();
63
64        Ok((input, body.into_pdu_body()))
65    }
66}
67
68pub(crate) fn force_id(input: &[u8]) -> IResult<&[u8], ForceId> {
69    let (input, force_id) = be_u8(input)?;
70    Ok((input, ForceId::from(force_id)))
71}
72
73pub(crate) fn entity_appearance(
74    entity_type: EntityType,
75) -> impl Fn(&[u8]) -> IResult<&[u8], EntityAppearance> {
76    move |input: &[u8]| {
77        let (input, appearance) = be_u32(input)?;
78
79        Ok((
80            input,
81            EntityAppearance::from_bytes(appearance, &entity_type),
82        ))
83    }
84}
85
86/// Parses the marking portion of an `EntityState` PDU into an `EntityMarking` struct.
87/// It will convert the parsed bytes (always 11 bytes are present in the PDU) to UTF-8, and
88/// strip trailing whitespace and any trailing non-alphanumeric characters. In case the marking is less
89/// than 11 characters, the trailing bytes are typically 0x00 in the PDU, which in UTF-8 is a control character.
90pub(crate) fn entity_marking(input: &[u8]) -> IResult<&[u8], EntityMarking> {
91    let mut buf: [u8; 11] = [0; 11];
92    let (input, marking_character_set) = be_u8(input)?;
93    let (input, ()) = nom::multi::fill(be_u8, &mut buf)(input)?;
94
95    let marking_character_set = EntityMarkingCharacterSet::from(marking_character_set);
96    let marking_string = sanitize_marking(&buf[..]);
97
98    Ok((
99        input,
100        EntityMarking {
101            marking_character_set,
102            marking_string,
103        },
104    ))
105}
106
107pub(crate) fn dr_parameters(input: &[u8]) -> IResult<&[u8], DrParameters> {
108    let (input, algorithm) = be_u8(input)?;
109    let algorithm = DeadReckoningAlgorithm::from(algorithm);
110
111    let (input, other_parameters) = dr_other_parameters(input, algorithm)?;
112
113    // // This match statement basically determines the value of the DrParametersType field for Euler and Quaternion variants
114    // let (input, other_parameters) = match algorithm {
115    //     DeadReckoningAlgorithm::StaticNonmovingEntity |
116    //         DeadReckoningAlgorithm::DRM_FPW_ConstantVelocityLowAccelerationLinearMotionEntity |
117    //         DeadReckoningAlgorithm::DRM_FVW_HighSpeedorManeuveringEntity |
118    //         DeadReckoningAlgorithm::DRM_FPB_SimilartoFPWexceptinBodyCoordinates |
119    //         DeadReckoningAlgorithm::DRM_FVB_SimilartoFVWexceptinBodyCoordinates => {
120    //         dr_other_parameters_euler(input)?
121    //     }
122    //     DeadReckoningAlgorithm::DRM_RPW_ConstantVelocityLowAccelerationLinearMotionEntitywithExtrapolationofOrientation |
123    //         DeadReckoningAlgorithm::DRM_RVW_HighSpeedorManeuveringEntitywithExtrapolationofOrientation |
124    //         DeadReckoningAlgorithm::DRM_RPB_SimilartoRPWexceptinBodyCoordinates |
125    //         DeadReckoningAlgorithm::DRM_RVB_SimilartoRVWexceptinBodyCoordinates => {
126    //         dr_other_parameters_quaternion(input)?
127    //     }
128    //     DeadReckoningAlgorithm::Other => {
129    //         dr_other_parameters_none(input)?
130    //     }
131    //     _ => {
132    //         dr_other_parameters_none(input)?
133    //     }
134    // };
135
136    let (input, acceleration) = vec3_f32(input)?;
137    let (input, velocity) = vec3_f32(input)?;
138
139    Ok((
140        input,
141        DrParameters {
142            algorithm,
143            other_parameters,
144            linear_acceleration: acceleration,
145            angular_velocity: velocity,
146        },
147    ))
148}
149
150#[allow(clippy::missing_errors_doc)]
151pub fn dr_other_parameters(
152    input: &[u8],
153    algorithm: DeadReckoningAlgorithm,
154) -> IResult<&[u8], DrOtherParameters> {
155    // This match statement basically determines the value of the DrParametersType field for Euler and Quaternion variants
156    let (input, other_parameters) = match algorithm {
157        DeadReckoningAlgorithm::StaticNonmovingEntity |
158        DeadReckoningAlgorithm::DRM_FPW_ConstantVelocityLowAccelerationLinearMotionEntity |
159        DeadReckoningAlgorithm::DRM_FVW_HighSpeedOrManeuveringEntity |
160        DeadReckoningAlgorithm::DRM_FPB_SimilarToFPWExceptInBodyCoordinates |
161        DeadReckoningAlgorithm::DRM_FVB_SimilarToFVWExceptInBodyCoordinates => {
162            dr_other_parameters_euler(input)?
163        }
164        DeadReckoningAlgorithm::DRM_RPW_ConstantVelocityLowAccelerationLinearMotionEntityWithExtrapolationOfOrientation |
165        DeadReckoningAlgorithm::DRM_RVW_HighSpeedOrManeuveringEntityWithExtrapolationOfOrientation |
166        DeadReckoningAlgorithm::DRM_RPB_SimilarToRPWExceptInBodyCoordinates |
167        DeadReckoningAlgorithm::DRM_RVB_SimilarToRVWExceptInBodyCoordinates => {
168            dr_other_parameters_quaternion(input)?
169        }
170        DeadReckoningAlgorithm::Other => {
171            dr_other_parameters_none(input)?
172        }
173        DeadReckoningAlgorithm::Unspecified(_) => {
174            dr_other_parameters_none(input)?
175        }
176    };
177
178    Ok((input, other_parameters))
179}
180
181pub(crate) fn dr_other_parameters_none(input: &[u8]) -> IResult<&[u8], DrOtherParameters> {
182    let (input, params) = take(15usize)(input)?;
183    Ok((input, DrOtherParameters::None(params.try_into().unwrap())))
184}
185
186pub(crate) fn dr_other_parameters_euler(input: &[u8]) -> IResult<&[u8], DrOtherParameters> {
187    let (input, _param_type) = be_u8(input)?;
188    let (input, _unused) = be_u16(input)?;
189    let (input, local_yaw) = be_f32(input)?;
190    let (input, local_pitch) = be_f32(input)?;
191    let (input, local_roll) = be_f32(input)?;
192    Ok((
193        input,
194        DrOtherParameters::LocalEulerAngles(DrEulerAngles {
195            local_yaw,
196            local_pitch,
197            local_roll,
198        }),
199    ))
200}
201
202pub(crate) fn dr_other_parameters_quaternion(input: &[u8]) -> IResult<&[u8], DrOtherParameters> {
203    let (input, _param_type) = be_u8(input)?;
204    let (input, nil) = be_u16(input)?;
205    let (input, x) = be_f32(input)?;
206    let (input, y) = be_f32(input)?;
207    let (input, z) = be_f32(input)?;
208    Ok((
209        input,
210        DrOtherParameters::WorldOrientationQuaternion(DrWorldOrientationQuaternion {
211            nil,
212            x,
213            y,
214            z,
215        }),
216    ))
217}
218
219#[cfg(test)]
220mod tests {
221    use crate::common::entity_state::model::EntityAppearance;
222    use crate::common::entity_state::parser::{entity_appearance, entity_marking};
223    use crate::common::model::VariableParameter;
224    use crate::common::model::{EntityType, PduBody};
225    use crate::common::parser::{location, parse_pdu, variable_parameter};
226    use crate::enumerations::*;
227    use crate::v6::entity_state::parser::entity_capabilities;
228
229    #[test]
230    fn parse_pdu_entity_state() {
231        let bytes: [u8; 208] = [
232            0x06, 0x01, 0x01, 0x01, 0x4e, 0xea, 0x3b, 0x60, 0x00, 0xd0, 0x00, 0x00, 0x01, 0xf4,
233            0x03, 0x84, 0x00, 0x0e, 0x01, 0x04, 0x01, 0x02, 0x00, 0x99, 0x32, 0x04, 0x04, 0x00,
234            0x01, 0x02, 0x00, 0x99, 0x32, 0x04, 0x04, 0x00, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00,
235            0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x41, 0x50, 0xc4, 0x1a, 0xde, 0xa4, 0xbe, 0xcc,
236            0x41, 0x50, 0xc9, 0xfa, 0x13, 0x3c, 0xf0, 0x5d, 0x41, 0x35, 0x79, 0x16, 0x9e, 0x7a,
237            0x16, 0x78, 0xbf, 0x3e, 0xdd, 0xfa, 0x3e, 0x2e, 0x36, 0xdd, 0x3f, 0xe6, 0x27, 0xc9,
238            0x00, 0x40, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
239            0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
240            0x00, 0x00, 0x00, 0x00, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
241            0x00, 0x00, 0x01, 0x45, 0x59, 0x45, 0x20, 0x31, 0x30, 0x20, 0x20, 0x20, 0x20, 0x20,
242            0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0c, 0x01, 0x3f, 0x80,
243            0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x0b,
244            0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
245            0x10, 0x0c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
246            0x00, 0x00, 0x11, 0x4d, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
247        ];
248
249        let pdu = parse_pdu(&bytes);
250        assert!(pdu.is_ok());
251        let pdu = pdu.unwrap();
252        assert_eq!(pdu.header.pdu_type, PduType::EntityState);
253        assert_eq!(pdu.header.pdu_length, 208u16);
254        if let PduBody::EntityState(pdu) = pdu.body {
255            assert_eq!(pdu.entity_id.simulation_address.site_id, 500u16);
256            assert_eq!(pdu.entity_id.simulation_address.application_id, 900u16);
257            assert_eq!(pdu.entity_id.entity_id, 14u16);
258            assert_eq!(pdu.force_id, ForceId::Friendly);
259            assert!(!pdu.variable_parameters.is_empty());
260            assert_eq!(pdu.variable_parameters.len(), 4usize);
261            assert_eq!(
262                pdu.entity_type,
263                EntityType {
264                    kind: EntityKind::Platform,
265                    domain: PlatformDomain::Air,
266                    country: Country::Netherlands_NLD_,
267                    category: 50,
268                    subcategory: 4,
269                    specific: 4,
270                    extra: 0
271                }
272            );
273
274            if let EntityAppearance::AirPlatform(appearance) = pdu.entity_appearance {
275                assert_eq!(appearance.paint_scheme, AppearancePaintScheme::UniformColor);
276                assert!(!appearance.propulsion_killed);
277                assert_eq!(appearance.damage, AppearanceDamage::NoDamage);
278                assert!(!appearance.is_smoke_emanating);
279                assert!(!appearance.is_engine_emitting_smoke);
280                assert_eq!(appearance.trailing_effects, AppearanceTrailingEffects::None);
281                assert_eq!(
282                    appearance.canopy_troop_door,
283                    AppearanceCanopy::SingleCanopySingleTroopDoorOpen
284                );
285                assert!(!appearance.landing_lights_on);
286                assert!(!appearance.navigation_lights_on);
287                assert!(!appearance.anticollision_lights_on);
288                assert!(!appearance.is_flaming);
289                assert!(!appearance.afterburner_on);
290                assert!(!appearance.is_frozen);
291                assert!(!appearance.power_plant_on);
292                assert_eq!(appearance.state, AppearanceEntityOrObjectState::Active);
293            } else {
294                panic!();
295            }
296
297            assert_eq!(pdu.dead_reckoning_parameters.algorithm, DeadReckoningAlgorithm::DRM_RVW_HighSpeedOrManeuveringEntityWithExtrapolationOfOrientation);
298            assert_eq!(pdu.entity_marking.marking_string, String::from("EYE 10"));
299            let capabilities: EntityCapabilities = pdu.entity_capabilities;
300            if let EntityCapabilities::AirPlatformEntityCapabilities(capabilities) = capabilities {
301                assert!(!capabilities.ammunition_supply);
302                assert!(!capabilities.fuel_supply);
303                assert!(!capabilities.recovery);
304                assert!(!capabilities.repair);
305            }
306            assert_eq!(pdu.variable_parameters.len(), 4);
307            let parameter_1 = pdu.variable_parameters.first().unwrap();
308            if let VariableParameter::Articulated(part) = parameter_1 {
309                assert_eq!(part.change_indicator, ChangeIndicator::from(0u8));
310                assert_eq!(part.attachment_id, 0u16);
311                assert_eq!(part.type_metric, ArticulatedPartsTypeMetric::Position);
312                assert_eq!(part.type_class, ArticulatedPartsTypeClass::LandingGear); // landing gear
313                assert_eq!(part.parameter_value, 1f32);
314            } else {
315                panic!();
316            }
317        } else {
318            panic!();
319        }
320    }
321
322    #[test]
323    fn parse_entity_location() {
324        let bytes: [u8; 24] = [
325            0x41, 0x50, 0xc4, 0x1a, 0xde, 0xa4, 0xbe, 0xcc, 0x41, 0x50, 0xc9, 0xfa, 0x13, 0x3c,
326            0xf0, 0x5d, 0x41, 0x35, 0x79, 0x16, 0x9e, 0x7a, 0x16, 0x78,
327        ];
328
329        let location = location(&bytes);
330        assert!(location.is_ok());
331        let (input, location) = location.unwrap();
332        assert_eq!(input.len(), 0);
333        assert_eq!(location.x_coordinate, 4_395_115.478_805_255);
334        assert_eq!(location.y_coordinate, 4_401_128.300_594_416);
335        assert_eq!(location.z_coordinate, 1_407_254.619_050_411_5);
336    }
337
338    #[test]
339    fn parse_marking_ascii() {
340        let bytes: [u8; 12] = [
341            0x01, 0x45, 0x59, 0x45, 0x20, 0x31, 0x30, 0x20, 0x20, 0x20, 0x20, 0x20,
342        ];
343
344        let marking = entity_marking(&bytes);
345        assert!(marking.is_ok());
346        let (input, marking) = marking.unwrap();
347        assert_eq!(
348            marking.marking_character_set,
349            EntityMarkingCharacterSet::ASCII
350        );
351        assert_eq!(marking.marking_string, "EYE 10");
352
353        assert!(input.is_empty());
354    }
355
356    #[test]
357    fn parse_marking_trailing_control_chars() {
358        let bytes: [u8; 12] = [
359            0x01, 0x45, 0x59, 0x45, 0x20, 0x31, 0x30, 0x00, 0x00, 0x00, 0x00, 0x00,
360        ];
361
362        let marking = entity_marking(&bytes);
363        assert!(marking.is_ok());
364        let (input, marking) = marking.unwrap();
365        assert_eq!(
366            marking.marking_character_set,
367            EntityMarkingCharacterSet::ASCII
368        );
369        assert_eq!(marking.marking_string, "EYE 10");
370
371        assert!(input.is_empty());
372    }
373
374    #[test]
375    fn parse_appearance_none() {
376        let input: [u8; 4] = [0x00, 0x00, 0x00, 0x00];
377        let entity_type = EntityType::default()
378            .with_kind(EntityKind::Platform)
379            .with_domain(PlatformDomain::Air);
380
381        let res = entity_appearance(entity_type)(&input);
382        assert!(res.is_ok());
383        let (input, appearance) = res.expect("value is Ok");
384
385        if let EntityAppearance::AirPlatform(appearance) = appearance {
386            assert_eq!(appearance.paint_scheme, AppearancePaintScheme::UniformColor);
387            assert!(!appearance.propulsion_killed);
388            assert_eq!(appearance.damage, AppearanceDamage::NoDamage);
389            assert!(!appearance.is_smoke_emanating);
390            assert!(!appearance.is_engine_emitting_smoke);
391            assert_eq!(appearance.trailing_effects, AppearanceTrailingEffects::None);
392            assert_eq!(
393                appearance.canopy_troop_door,
394                AppearanceCanopy::NotApplicable
395            );
396            assert!(!appearance.landing_lights_on);
397            assert!(!appearance.is_flaming);
398        } else {
399            panic!();
400        }
401        assert!(input.is_empty());
402    }
403
404    #[test]
405    fn parse_appearance_emitting_engine_smoke() {
406        let input: [u8; 4] = [0x06, 0x00, 0x00, 0x00];
407        let entity_type = EntityType::default()
408            .with_kind(EntityKind::Platform)
409            .with_domain(PlatformDomain::Air);
410
411        let res = entity_appearance(entity_type)(&input);
412        assert!(res.is_ok());
413        let (input, appearance) = res.expect("value is Ok");
414
415        if let EntityAppearance::AirPlatform(appearance) = appearance {
416            assert!(appearance.is_smoke_emanating);
417            assert!(appearance.is_engine_emitting_smoke);
418        } else {
419            panic!();
420        }
421        assert!(input.is_empty());
422    }
423
424    #[test]
425    fn parse_entity_capabilities_none() {
426        let input: [u8; 4] = [0x00, 0x00, 0x00, 0x00];
427
428        let res = entity_capabilities(&input);
429        assert!(res.is_ok());
430        let (input, capabilities) = res.expect("value is Ok");
431        assert!(!capabilities.ammunition_supply);
432        assert!(!capabilities.fuel_supply);
433        assert!(!capabilities.recovery);
434        assert!(!capabilities.repair);
435
436        assert!(input.is_empty());
437    }
438
439    #[test]
440    fn parse_articulated_parameter_gun1_azimuth() {
441        let input: [u8; 16] = [
442            0x00, // u8; type articulated
443            0x00, // u8; no change
444            0x00, 0x00, // u16; 0 value attachment id
445            0x00, 0x00, // u32; type variant metric - 11 - azimuth
446            0x10, 0x0b, // type variant high bits - 4096 - primary gun 1
447            0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
448        ]; // f64 - value 0
449
450        let parameter = variable_parameter(&input);
451        assert!(parameter.is_ok());
452        let (input, parameter) = parameter.expect("should be Ok");
453
454        if let VariableParameter::Articulated(articulated_part) = parameter {
455            assert_eq!(
456                articulated_part.change_indicator,
457                ChangeIndicator::from(0u8)
458            );
459            assert_eq!(articulated_part.attachment_id, 0);
460            assert_eq!(
461                articulated_part.type_class,
462                ArticulatedPartsTypeClass::PrimaryTurretNumber1
463            );
464            assert_eq!(
465                articulated_part.type_metric,
466                ArticulatedPartsTypeMetric::Azimuth
467            );
468        } else {
469            panic!();
470        }
471
472        assert!(input.is_empty());
473    }
474
475    #[test]
476    fn parse_articulated_parameter_landing_gear_down() {
477        let input: [u8; 16] = [
478            0x00, // u8; type articulated
479            0x00, // u8; no change
480            0x00, 0x00, // u16; 0 value attachment id
481            0x00, 0x00, // u32; type variant metric - 11 - position
482            0x0C, 0x01, // type variant high bits - 3072 - landing gear
483            0x3F, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
484        ]; // f32 - value '1' and 4 bytes padding
485
486        let parameter = variable_parameter(&input);
487        assert!(parameter.is_ok());
488        let (input, parameter) = parameter.expect("should be Ok");
489        if let VariableParameter::Articulated(articulated_part) = parameter {
490            assert_eq!(
491                articulated_part.change_indicator,
492                ChangeIndicator::from(0u8)
493            );
494            assert_eq!(articulated_part.attachment_id, 0);
495            assert_eq!(
496                articulated_part.type_class,
497                ArticulatedPartsTypeClass::LandingGear
498            );
499            assert_eq!(
500                articulated_part.type_metric,
501                ArticulatedPartsTypeMetric::Position
502            );
503            assert_eq!(articulated_part.parameter_value, 1f32);
504        } else {
505            panic!();
506        }
507
508        assert!(input.is_empty());
509    }
510}
dis_rs/common/entity_state/parser.rs

dis_rs/common/entity_state/
parser.rs
