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

/*
 * File: focal_point_gain.rs
 * Project: src
 * Created Date: 15/11/2019
 * Author: Shun Suzuki
 * -----
 * Last Modified: 22/05/2020
 * Modified By: Shun Suzuki (suzuki@hapis.k.u-tokyo.ac.jp)
 * -----
 * Copyright (c) 2019 Hapis Lab. All rights reserved.
 *
 */

use autd_geometry::consts::NUM_TRANS_IN_UNIT;
use autd_geometry::Geometry;
use autd_geometry::Vector3;

use crate::consts::ULTRASOUND_WAVELENGTH;
use crate::convert_to_pwm_params;
use crate::Gain;

/// Gain to produce single focal point
pub struct FocalPointGain {
    point: Vector3,
    amp: u8,
    data: Option<Vec<u8>>,
}

impl FocalPointGain {
    /// Generate FocalPointGain.
    ///
    /// # Arguments
    ///
    /// * `point` - focal point.
    ///
    pub fn create(point: Vector3) -> Box<FocalPointGain> {
        FocalPointGain::create_with_amp(point, 0xff)
    }

    /// Generate FocalPointGain.
    ///
    /// # Arguments
    ///
    /// * `point` - focal point.
    /// * `amp` - amplitude of the focus.
    ///
    pub fn create_with_amp(point: Vector3, amp: u8) -> Box<FocalPointGain> {
        Box::new(FocalPointGain {
            point,
            amp,
            data: None,
        })
    }
}

impl Gain for FocalPointGain {
    fn get_data(&self) -> &[u8] {
        assert!(self.data.is_some());
        match &self.data {
            Some(data) => data,
            None => panic!(),
        }
    }

    fn build(&mut self, geometry: &Geometry) {
        if self.data.is_some() {
            return;
        }

        let num_devices = geometry.num_devices();
        let num_trans = NUM_TRANS_IN_UNIT * num_devices;
        let mut data = Vec::with_capacity(num_trans * 2);

        let point = self.point;
        let amp = self.amp;

        for i in 0..num_trans {
            let trp = geometry.position(i);
            let dist = (trp - point).norm();
            let phase = (dist % ULTRASOUND_WAVELENGTH) / ULTRASOUND_WAVELENGTH;
            let phase = (255.0 * (1.0 - phase)) as u8;
            let amp: u8 = amp;
            let (d, s) = convert_to_pwm_params(amp, phase);
            data.push(s);
            data.push(d);
        }
        self.data = Some(data);
    }
}