1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111

/*
 * File: utils.rs
 * Project: src
 * Created Date: 27/05/2021
 * Author: Shun Suzuki
 * -----
 * Last Modified: 21/07/2021
 * Modified By: Shun Suzuki (suzuki@hapis.k.u-tokyo.ac.jp)
 * -----
 * Copyright (c) 2021 Hapis Lab. All rights reserved.
 *
 */

use std::f64::consts::PI;

/// Adjust amplitude to duty ratio of Pulse Width Modulation.
pub fn adjust_amp(amp: f64) -> u8 {
    let d = amp.asin() / PI; // duty (0 ~ 0.5)
    (510.0 * d) as u8
}

#[cfg(feature = "phase_inverted")]
pub fn to_phase(phase: f64) -> u8 {
    (((phase * 256.0).round() as i32) & 0xFF) as u8
}

#[cfg(not(feature = "phase_inverted"))]
pub fn to_phase(phase: f64) -> u8 {
    let phase = (((phase * 256.0).round() as i32) & 0xFF) as u8;
    0xFF - phase
}

pub fn pack_to_u16(high: u8, low: u8) -> u16 {
    let low = (low as u16) & 0x00FF;
    let high = ((high as u16) << 8) & 0xFF00;
    high | low
}

#[allow(clippy::excessive_precision, clippy::unreadable_literal)]
static DIR_COEF_A: &[f64] = &[
    1.0,
    1.0,
    1.0,
    0.891250938,
    0.707945784,
    0.501187234,
    0.354813389,
    0.251188643,
    0.199526231,
];

#[allow(clippy::excessive_precision, clippy::unreadable_literal)]
static DIR_COEF_B: &[f64] = &[
    0.,
    0.,
    -0.00459648054721,
    -0.0155520765675,
    -0.0208114779827,
    -0.0182211227016,
    -0.0122437497109,
    -0.00780345575475,
    -0.00312857467007,
];

#[allow(clippy::excessive_precision, clippy::unreadable_literal)]
static DIR_COEF_C: &[f64] = &[
    0.,
    0.,
    -0.000787968093807,
    -0.000307591508224,
    -0.000218348633296,
    0.00047738416141,
    0.000120353137658,
    0.000323676257958,
    0.000143850511,
];

#[allow(clippy::excessive_precision, clippy::unreadable_literal)]
static DIR_COEF_D: &[f64] = &[
    0.,
    0.,
    1.60125528528e-05,
    2.9747624976e-06,
    2.31910931569e-05,
    -1.1901034125e-05,
    6.77743734332e-06,
    -5.99548024824e-06,
    -4.79372835035e-06,
];

#[allow(clippy::many_single_char_names)]
pub fn directivity_t4010a1(theta_deg: f64) -> f64 {
    let mut theta_deg = theta_deg.abs();

    while theta_deg > 90.0 {
        theta_deg = (180.0 - theta_deg).abs();
    }

    let i = (theta_deg / 10.0).ceil() as usize;

    if i == 0 {
        1.0
    } else {
        let a = DIR_COEF_A[i - 1];
        let b = DIR_COEF_B[i - 1];
        let c = DIR_COEF_C[i - 1];
        let d = DIR_COEF_D[i - 1];
        let x = theta_deg - (i as f64 - 1.0) * 10.0;
        a + b * x + c * x * x + d * x * x * x
    }
}