TimeOfFlightMonitor.cs

using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class TimeOfFlightMonitor : MonoBehaviour {

    // Microphone
    string micDeviceName = "Headset mic (Realtek High Definition Audio)";
    AudioClip micClip;
    int lastSamplePos;
    const int MIC_SAMPLE_RATE = 44100; // Sample rate in Hz

    float NONMAX_SUPPRESS_TIME_S = 0.025f; // when finding peaks in audio for event detection, do not consider peaks closer together than this amount

    float shotTimeoutSeconds = 0.50f; // If we're tracking a shot longer than this, we probably missed something & should reset

    public GameObject trackedDevice;

    [Range(10.0f,50.0f)]
    public float shotVelocity; // Velocity of the bullet in m/s.  

    // State machine & variables for tracking airsoft shots and time of flight
    enum ShotTrackState
    {
        Idle = 0,
        SpringFired,
        BarrelExited,
        Impacted
    };

    ShotTrackState currState;
    float lastSpringFiredTime;
    float lastBarrelExitedTime;
    float lastImpactedTime;

    // Data for the reconstructed point cloud
    class AirsoftPoint
    {
        public Vector3 shotPosition;
        public Vector3 shotDirection;
        public float timeOfFlight;
    }

    List<AirsoftPoint> pointCloud;  // Contains info about shots/impacts

    List<GameObject> renderedCloud; // The actual things that get rendered

    GameObject makeNewPoint()
    {
        GameObject sphere = GameObject.CreatePrimitive(PrimitiveType.Sphere);
        sphere.transform.localScale = new Vector3(0.01f, 0.01f, 0.01f);
        return sphere;
    }

	void Start () {
        // Start the microphone
        foreach (string device in Microphone.devices)
        {
            Debug.Log("Available mic: " + device);
        }
        micClip = Microphone.Start(micDeviceName, false, 3600, 44100);
        Debug.Log("Started listening to device " + micDeviceName);
        lastSamplePos = 0;
        currState = ShotTrackState.Idle;

        pointCloud = new List<AirsoftPoint>();
        renderedCloud = new List<GameObject>();
	}
	
	// Update is called once per frame
	void Update () {
        int currSamplePos = Microphone.GetPosition(micDeviceName);
        float tCurrSample = Time.time;  // Oh, for a microphone that recorded proper timestamps... ah well.

        if(currState == ShotTrackState.Impacted)
        {
            if (tCurrSample - lastImpactedTime < shotTimeoutSeconds*2)
                return;
        }

        currState = ShotTrackState.Idle;

        // ----------- Process microphone data -----------
        int numSamples = (int)(shotTimeoutSeconds * MIC_SAMPLE_RATE);
        if(currSamplePos - numSamples > 0)
        { 
            float[] samples = new float[numSamples];
            micClip.GetData(samples, currSamplePos - numSamples);

            // We are looking for sharp peaks - take the absolute value of the numerical derivative.  
            float[] dSamples = new float[numSamples];
            dSamples[0] = 0;
            float mean = 0;
            for(int i = 1; i < numSamples; i++)
            {
                //dSamples[i] = Mathf.Abs(samples[i] - samples[i - 1]);
                dSamples[i] = Mathf.Abs(samples[i]);
                mean += dSamples[i];
            }
            mean /= numSamples;

            // Iteratively find peaks separated by at least NONMAX_SUPPRESS_TIME_S
            List<float> peakTimes = new List<float>();

            while(true)
            {
                float peakVal = 0.0f;
                int peakIdx = -1;
                for(int i = 0; i < numSamples; i++)
                {
                    if(peakVal < dSamples[i])
                    {
                        peakVal = dSamples[i];
                        peakIdx = i;
                    }
                }

                if (peakVal > 0.5)
                {
                    // convert sample index to time & keep track of this event
                    float peakTime = tCurrSample - ((float)(numSamples - peakIdx)) / (float)(MIC_SAMPLE_RATE);
                    peakTimes.Add(peakTime);

                    // enforce NONMAX_SUPPRESS_TIME_S
                    int nSuppressSamples = (int)(NONMAX_SUPPRESS_TIME_S * MIC_SAMPLE_RATE + 0.5);
                    int startSuppressWindow = peakIdx - nSuppressSamples;
                    int endSuppressWindow = peakIdx + nSuppressSamples;

                    startSuppressWindow = startSuppressWindow < 0 ? 0 : startSuppressWindow;
                    endSuppressWindow = endSuppressWindow >= numSamples ? numSamples - 1 : endSuppressWindow;

                    for (int i = startSuppressWindow; i <= endSuppressWindow; i++)
                        dSamples[i] = 0;
                } else
                {
                    // Nothing else interesting here
                    break;
                }
            }
            
            // If we have any events, move through the state machine & record if needed.
            // We need at least 3 peaks to have properly measured a shot.
            if(peakTimes.Count >= 3)
            {
                peakTimes.Sort();
                string detectedPeaks = "";
                foreach (float eventTime in peakTimes)
                    detectedPeaks = detectedPeaks + " " + eventTime;

                Debug.Log("Detected peaks: " + detectedPeaks);

                foreach(float eventTime in peakTimes)
                {
                    switch(currState)
                    {
                        case ShotTrackState.Idle:
                            lastSpringFiredTime = eventTime;
                            Debug.Log("Detected spring fired at " + lastSpringFiredTime);
                            currState = ShotTrackState.SpringFired;
                            break;

                        case ShotTrackState.SpringFired:

                            //if (Mathf.Abs(eventTime - lastSpringFiredTime - 0.0345f) < 0.005f)
                            //{
                                Debug.Log("Detected barrel exit at T+" + 1000*(eventTime - lastSpringFiredTime));
                                lastBarrelExitedTime = eventTime;
                                currState = ShotTrackState.BarrelExited;
                            //} else
                            //{
                                //Debug.Log("Rejecting barrel exit at T+" + 1000*(eventTime - lastSpringFiredTime) + " (error: " + 1000*Mathf.Abs(eventTime - lastSpringFiredTime - 0.033f) + ")");
                               // lastSpringFiredTime = eventTime;
                                //currState = ShotTrackState.SpringFired;
                            //}

                            break;

                        case ShotTrackState.BarrelExited:
                            lastImpactedTime = eventTime;
                            Debug.Log("Detected shot: npeaks=" + peakTimes.Count +
                                " lastSpringFiredTime " + lastSpringFiredTime + 
                                " +barrel " + (1000*(lastBarrelExitedTime - lastSpringFiredTime)) + 
                                " +impact " + (1000*(lastImpactedTime - lastBarrelExitedTime)));
                            currState = ShotTrackState.Impacted;

                            // Record pose.  (TODO back-predict poses to estimated barrel exit time?) 
                            AirsoftPoint pt = new AirsoftPoint();
                            pt.shotPosition = trackedDevice.transform.position;
                            pt.shotDirection = trackedDevice.transform.forward;
                            pt.timeOfFlight = lastImpactedTime - lastBarrelExitedTime;
                            pointCloud.Add(pt);
                            renderedCloud.Add(makeNewPoint());
                            break;
                    }
                }
            }

            // Place points at impact locations
            for(int i = 0; i < pointCloud.Count; i++)
            {
                renderedCloud[i].transform.position = pointCloud[i].shotPosition + shotVelocity * pointCloud[i].timeOfFlight * pointCloud[i].shotDirection;
            }

        }
	}
}