public void SetBearing(float x, float y, float z)
{
//Debug.Log("SetRotation: "+ x.ToString() + "," + y.ToString() + "," +z.ToString());
//TODO: alusta bearing parametrit.
// fixedupdaten autopilotstate.bearingTurn caseen suoritetaan kääntörutiin.
//var xDeg = SDegree(x);
autopilotState = autoPilotStates.bearingTurn;
//targetBearing = new Vector3(x, y, z);
SDegree toX = new SDegree(x - transform.rotation.eulerAngles.x);
//var targetDir = transform.rotation.eulerAngles + targetBearing;
deltaRotation = Quaternion.FromToRotation(transform.forward, targetBearing);
toDirQ = Quaternion.FromToRotation(Vector3.forward, targetBearing);
toDir = toDirQ.eulerAngles;
DeltadirX = toDir.x - transform.rotation.eulerAngles.x;
DeltadirY = toDir.y - transform.rotation.eulerAngles.y;
DeltadirZ = toDir.z - transform.rotation.eulerAngles.z;
localAngularVelocity = GetComponent <Rigidbody>().transform.InverseTransformDirection(GetComponent <Rigidbody>().angularVelocity);
float vax = localAngularVelocity.x;
float vay = localAngularVelocity.y;
float vaz = localAngularVelocity.z;
}
void FixedUpdate()
{
myVelocity = Mathf.Sqrt(Mathf.Pow(myVelX, 2) + Mathf.Pow(myVelY, 2) + Mathf.Pow(myVelZ, 2));
// vector3:n tallennettu pyörimisnopeus (localAngularVelocity.x, localAngularVelocity.y, localAngularVelocity.z)
localAngularVelocity = GetComponent <Rigidbody>().transform.InverseTransformDirection(GetComponent <Rigidbody>().angularVelocity);
// Jos ollaan jo nykyisessä waypointissa, niin suunnataan nokka seuraavaan
if (isAtWaypoint(Waypoints[currentWaypoint]) && myVelocity < 0.1)
{
if (currentWaypoint < Waypoints.Count)
{
currentWaypoint++;
}
//Stop ();
autopilotState = autoPilotStates.idle;
if (currentWaypoint < Waypoints.Count)
{
autopilotState = autoPilotStates.setDirection;
var currentWp = Waypoints[currentWaypoint];
originalDistanceToDestination = Mathf.Sqrt(Mathf.Pow((float)(currentWp.X - myX), 2) + Mathf.Pow((float)(currentWp.Y - myY), 2) + Mathf.Pow((float)(currentWp.Z - myZ), 2));
distanceToDestination = new Vector3((float)(currentWp.X - myX), (float)(currentWp.Y - myY), (float)(currentWp.Z - myZ));
}
else
{
currentWaypoint = 0;
}
}
switch (autopilotState)
{
case autoPilotStates.setDirection:
if (SetDirection(Waypoints[currentWaypoint]) == true)
{
autopilotState = autoPilotStates.prograde;
}
break;
case autoPilotStates.prograde:
var currentWp = Waypoints[currentWaypoint];
distanceToDestination = new Vector3((float)(currentWp.X - myX), (float)(currentWp.Y - myY), (float)(currentWp.Z - myZ));
// retroStartDist on maksimietäisyys, jolla pystytään pysäyttämään alus.
retroStartDist = new Vector3((GetComponent <Rigidbody>().mass *Mathf.Pow(myVelX, 2)) / (2 * myThrust),
(GetComponent <Rigidbody>().mass *Mathf.Pow(myVelY, 2)) / (2 * myThrust),
(GetComponent <Rigidbody>().mass *Mathf.Pow(myVelZ, 2)) / (2 * myThrust));
if (distanceToDestination.magnitude > (originalDistanceToDestination * 0.65))
{
addThrust(Vector3.forward);
}
if (distanceToDestination.magnitude <= retroStartDist.magnitude)
{
autopilotState = autoPilotStates.retrograde;
}
break;
case autoPilotStates.retrograde:
// retroStartDist on maksimietäisyys, jolla pystytään pysäyttämään alus.
//retroStartDist = (GetComponent<Rigidbody>().mass * Mathf.Pow(myVelocity, 2) ) / (2 * myThrust);
if (distanceToDestination.x <= retroStartDist.x && Mathf.Abs(myVelX) > 0.1)
{
StopX();
}
if (distanceToDestination.y <= retroStartDist.y && Mathf.Abs(myVelY) > 0.1)
{
StopY();
}
if (distanceToDestination.z <= retroStartDist.z && Mathf.Abs(myVelZ) > 0.1)
{
StopZ();
}
else if (isAtWaypoint(Waypoints[currentWaypoint]) == false && myVelocity < 0.2)
{
autopilotState = autoPilotStates.setDirection;
}
if (isAtWaypoint(Waypoints[currentWaypoint]) == true && myVelocity < 0.2)
{
autopilotState = autoPilotStates.idle;
}
break;
case autoPilotStates.bearingTurn:
break;
case autoPilotStates.idle:
default:
break;
}
/*
* Vector3 refVelVec = new Vector3 (myVelX, myVelY, myVelZ);
* Quaternion rotations = GetComponent<Rigidbody>().rotation;
* Matrix4x4 m = Matrix4x4.TRS(Vector3.zero, rotations, Vector3.one);
* myLocalVel = m.MultiplyPoint3x4(refVelVec);
*/
Vector3 refVelVec = new Vector3(myVelX, myVelY, myVelZ);
myLocalVel = GetComponent <Rigidbody>().transform.InverseTransformDirection(refVelVec);
myX += myVelX * Time.fixedDeltaTime;
myY += myVelY * Time.fixedDeltaTime;
myZ += myVelZ * Time.fixedDeltaTime;
UpdatePosition();
}
