// Called every frame while in playback to update the PlaybackFrisbee position void UpdatePositionFromList() { if (animIndex < throwBuffer.Count - 1 && animIndex >= 0) { if (throwBuffer[animIndex] != null) { FrisbeeLocation location = throwBuffer[animIndex]; frisbeeModel.transform.localRotation = location.rot; frisbeeModel.transform.localPosition = location.pos; //current = location; } rateTimer += Time.deltaTime * speed; animIndex = getListIndexFromTime(throwBuffer, rateTimer, animIndex); } else { animIndex = 1; rateTimer = 0F; pauseUntil = Time.time + 1.0F; } /*if (Time.frameCount % 100 == 0) * Debug.Log("IDX: " + animIndex);*/ }
// Handles the backtracking and saves the part of the throw that happened // before the detection occurred void HandleThrow() { if (useOptitrackTimestamp) { throwStartTime = OptitrackHiResTimer.Now().SecondsSince(customRB.zeroTime) - throwBacktrackingTime; } else { throwStartTime = Time.time - throwBacktrackingTime; } int firstThrowIndex = getBufferIndexFromTime(captureBuffer, throwStartTime); throwMode = 2; Debug.Log("Throw just began!"); //get first FrisbeeLocation in throw FrisbeeLocation temp = captureBuffer.Get(firstThrowIndex); FrisbeeLocation prev = null; throwStartPos = temp.pos; throwStartTime = temp.time; //reinitialize throwBuffer to empty list throwBuffer = new List <FrisbeeLocation>(); //Transfer previous throwBacktrackingTime seconds of data to throwBuffer //to make sure that the beginning of the throw is not lost //TODO: maybe add a more sophisticated backtracking algorithm that is based on velocity for (int i = firstThrowIndex; i < captureBuffer.Count; i++) { temp = captureBuffer.Get(i); //manually initialize first timestamp to zero if (i == firstThrowIndex) { throwBuffer.Add(new FrisbeeLocation(temp.rot, temp.pos, 0F, 0F, 0F, temp.wasSeen)); } else { if (i - firstThrowIndex >= 2) { prev = throwBuffer[throwBuffer.Count - 2]; } else { prev = throwBuffer[throwBuffer.Count - 1]; } float currentThrowTime = temp.time - throwStartTime; //rest of the timestamps will increment from zero //add backtracked points to throwBuffer throwBuffer.Add(new FrisbeeLocation(temp.rot, temp.pos, currentThrowTime, 0F, 0F, temp.wasSeen)); } //Debug.Log("ThrowBuffer: " + throwBuffer[throwBuffer.Count - 1].rotSpeed.ToString()); } }
/* * Buffers BUFFERCAPACITY number of previous FrisbeeLocations * */ void Update() { if (Input.GetKeyDown("r")) { throwMode = 0; } Vector3 currentPosition = new Vector3(trackingTarget.position.x, trackingTarget.position.y, trackingTarget.position.z); //add current FrisbeeLocation to back of queue //buffer has not yet reached full capacity bool isSeen = customRB.isSeen(); float currTime = Time.time; if (isSeen && useOptitrackTimestamp) { currTime = customRB.time(); } if (captureBuffer.Count < BUFFERCAPACITY - 1) { captureBuffer.Add(new FrisbeeLocation(trackingTarget.localRotation, trackingTarget.localPosition, currTime, isSeen)); } else //buffer has reached BUFFERCAPACITY { captureBuffer.RemoveFront(); //remove oldest FrisbeeLocation captureBuffer.Add(new FrisbeeLocation(trackingTarget.localRotation, trackingTarget.localPosition, currTime, isSeen)); } //0 = playback , 1 = waiting for throw + playback , 2 = throw in progress //Buffer first 2.5 seconds if (Time.time > scriptStartTime + 2.5F) { HandleModeChanges(currentPosition); //change mode when appropriate } if (Throwing()) { // Throw has begun, add FrisbeeLocations to throwBuffer //Throw has begun, throwBuffer timestamps increment from zero FrisbeeLocation temp = captureBuffer.Get(captureBuffer.Count - 1); float currentThrowTime = temp.time - throwStartTime; throwBuffer.Add(new FrisbeeLocation(temp.rot, temp.pos, currentThrowTime, 0F, 0F, temp.wasSeen)); } SetModeText(); }
//Detect if z-distance is greater than ending threshold (virtual wall) //Calculate speeds void HandleEnd(Vector3 position) { if (position.z > threshold.ending) { Debug.Log("hit max throw z-distance at: " + position.ToString("F3")); if (throwBuffer.Count > speedCalculationFrames * 2 + 10) { for (int i = speedCalculationFrames; i < throwBuffer.Count - speedCalculationFrames; i++) { FrisbeeLocation temp = throwBuffer[i + speedCalculationFrames]; FrisbeeLocation prev = throwBuffer[i - speedCalculationFrames]; float fSpeed = Vector3.Distance(temp.pos, prev.pos) / (temp.time - prev.time); //this is experimental, we are not absolutely sure if this is the correct way to calculate rpm float a = (Quaternion.Inverse(prev.rot) * temp.rot).eulerAngles.y; float b = (Quaternion.Inverse(temp.rot) * prev.rot).eulerAngles.y; float rSpeed = Mathf.Abs((Mathf.Min(a, b) / 360F) / (temp.time - prev.time)); rSpeed = rSpeed * 60F; throwBuffer[i].forwardSpeed = fSpeed; throwBuffer[i].rotSpeed = rSpeed; } //set beginning and end speeds for (int i = 0; i < speedCalculationFrames; i++) { throwBuffer[i].forwardSpeed = throwBuffer[speedCalculationFrames].forwardSpeed; throwBuffer[i].rotSpeed = throwBuffer[speedCalculationFrames].rotSpeed; } for (int i = throwBuffer.Count - speedCalculationFrames; i < throwBuffer.Count; i++) { throwBuffer[i].forwardSpeed = throwBuffer[throwBuffer.Count - speedCalculationFrames - 1].forwardSpeed; throwBuffer[i].rotSpeed = throwBuffer[throwBuffer.Count - speedCalculationFrames - 1].rotSpeed; } } throwMode = 0; nextThrow = Time.time + throwRate; } }
// Updates the RecordingFrisbee location from csv file. Scaling and offset left for manual adjustment void UpdatePosition() { if (animIndex < locationList.Count) { if (locationList[animIndex] != null) { FrisbeeLocation location = locationList[animIndex]; frisbeeModel.transform.localRotation = location.rot; frisbeeModel.transform.localPosition = Vector3.Scale(location.pos - new Vector3(0F, 0F, 0F), posScale); } animIndex = (int)(rateTimer / (1 / 100F)); //presumes 100fps rateTimer += Time.deltaTime * speed; } else { animIndex = 0; rateTimer = 0F; moving = false; } }
// Simulates the frisbee with empirically based model in the z and y-axis, and simple constant velocity on the x-axis void simulateFrisbee() { FrisbeeLocation cutoff = null; int cutoffIndex = 0; //Find a cutoff point where the frisbee is likely to still be in mid-flight foreach (FrisbeeLocation loc in throwBuffer) { //cutoff default simulationBegin is 0.7 times the travel distance from throw start to throw end if (loc.pos.z - throwBuffer[0].pos.z > simulationBegin * (throwBuffer[throwBuffer.Count - 1].pos.z - throwBuffer[0].pos.z)) { cutoff = loc; break; } cutoffIndex++; } if (cutoff != null) { //Calculate the velocities and angle to ground plane FrisbeeLocation prevLoc = throwBuffer[cutoffIndex - 4]; double vz0 = (cutoff.pos.z - prevLoc.pos.z) / (cutoff.time - prevLoc.time); double vy0 = (cutoff.pos.y - prevLoc.pos.y) / (cutoff.time - prevLoc.time); double vx0 = (cutoff.pos.x - prevLoc.pos.x) / (cutoff.time - prevLoc.time); Vector3 forwardDirection = cutoff.pos - prevLoc.pos; forwardDirection.Normalize(); float pitch = Vector3.Angle(forwardDirection, Vector3.ProjectOnPlane(forwardDirection, Vector3.up)); //window of numerical integration is set to 0.001s List <FrisbeeLocation> simulated = pred.simulate3D(cutoff.pos.x, cutoff.pos.y, cutoff.pos.z, vx0, vy0, vz0, pitch, 0.001F); //debug /*Debug.Log("Simulation len: " + simulated.Count); * Debug.Log("ThrowBuffer len: " + throwBuffer.Count); * Debug.Log("Cutoff and prev y-pos: " + cutoff.pos.y + " " + prevLoc.pos.y); * Debug.Log("Cutoff and prevloc timestamps" + cutoff.time + " " + prevLoc.time); * Debug.Log("Simulation parameters: " + cutoff.pos.y + " " + vz0 + " " + vy0 + " " + pitch);*/ simulationTrail.Create(simulated); endingSpeedText.text = "Ending speed before simulation: " + cutoff.forwardSpeed.ToString("F1") + " m/s"; } }
// Update is called once per frame void Update() { if (Input.GetKeyDown("space")) { moving = true; animIndex = 0; rateTimer = 0F; beginTime = Time.time; } //Begin steady else if (Time.time < beginTime + 2.5F) { FrisbeeLocation location = locationList[0]; frisbeeModel.transform.localRotation = location.rot; frisbeeModel.transform.localPosition = Vector3.Scale(location.pos - new Vector3(0F, 0F, 0F), posScale); } //Update the position from locationList else if (moving) { UpdatePosition(); } }