private StateMessage GetState(Inputs input)
{
StateMessage state_msg = new StateMessage();
state_msg.setPosition(cube_rigidbody.position);
state_msg.setRotation(cube_rigidbody.rotation);
state_msg.setVelocity(cube_rigidbody.velocity);
state_msg.setVelocity_Ang(cube_rigidbody.angularVelocity);
state_msg.setInput(input);
state_msg.tick_number = tick_number;
// Debug.Log("vel " + state_msg.velocity);
return(state_msg);
}
private void SimulateInterpolationHermite4(float interpolationFraction)
{
//
if (InterpolationBuffer.Count >= 3 && !interpolationg)
{
SM1.setPosition(cube_rigidbody.position);
SM1.setRotation(cube_rigidbody.rotation);
SM1.setVelocity_Ang(cube_rigidbody.angularVelocity);
SM2 = InterpolationBuffer.Dequeue();
SM3 = InterpolationBuffer.Dequeue();
SM4 = InterpolationBuffer.Dequeue();
journeyLength = interpolationFraction;
Debug.Log("Interpolation starts sm1 " + SM1.getPosition() + "sm2 " + SM2.getPosition() + "sm3 " + SM3.getPosition() + "sm4 " + SM4.getPosition() + "buffer size " + InterpolationBuffer.Count);
interpolationg = true;
}
if (interpolationg)
{
Vector3 IntPos = Hermite4(SM1.getPosition(), SM2.getPosition(), SM3.getPosition(), SM4.getPosition(), journeyLength);
//Debug.Log("Interpolation in sm1 " + SM1.getPosition() + "sm2 " + SM2.getPosition() + "sm3 " + SM3.getPosition() + "sm4 " + SM4.getPosition());
Debug.Log("my postion " + cube_rigidbody.position + " hermite postion " + IntPos + " goal " + SM4.getPosition() + " fract of journey " + journeyLength);
cube_rigidbody.position = IntPos;
if (journeyLength >= 1f)
{
interpolationg = false;
}
else if (journeyLength >= 0.66f)
{
cube_rigidbody.rotation = Quaternion.Slerp(SM3.getRotation(), SM4.getRotation(), ((journeyLength - 0.66f) / 0.33f));
cube_rigidbody.velocity = SM3.getVelocity();
cube_rigidbody.angularVelocity = SM3.getVelocity_Angular();
}
else if (journeyLength >= 0.33f)
{
cube_rigidbody.rotation = Quaternion.Slerp(SM2.getRotation(), SM3.getRotation(), ((journeyLength - 0.33f) / 0.33f));
cube_rigidbody.velocity = SM2.getVelocity();
cube_rigidbody.angularVelocity = SM2.getVelocity_Angular();
}
else if (journeyLength >= 0.0f)
{
cube_rigidbody.rotation = Quaternion.Slerp(SM1.getRotation(), SM2.getRotation(), (journeyLength / 0.33f));
cube_rigidbody.velocity = SM1.getVelocity();
cube_rigidbody.angularVelocity = SM1.getVelocity_Angular();
}
journeyLength += interpolationFraction;
}
}
