// 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();
}
}
