void NavUpdate(ScriptableRenderContext context, Camera[] cameras)
{
_unityPosition = ConventionTransforms.PositionUnityToNED(gameObject.transform.position);
_unityOrientation = ConventionTransforms.OrientationUnityToNED(gameObject.transform.rotation);
// Set Navigation Position and orientation to the same value as the Unity position and orientation
// TODO: This should be done in a own function
_navPosition.X = _unityPosition.x;
_navPosition.Y = _unityPosition.y;
_navPosition.Z = _unityPosition.z;
_navOrientation.X = _unityOrientation.x;
_navOrientation.Y = _unityOrientation.y;
_navOrientation.Z = _unityOrientation.z;
_navOrientation.W = _unityOrientation.w;
// TODO: In the future this should either be estimated from position and time
// or this should be retreived from a Rigidbody component, depending on which
// type of simulation that is running.
_navLinearVelocity.X = 0.0f;
_navLinearVelocity.Y = 0.0f;
_navLinearVelocity.Z = 0.0f;
_navAngularVelocity.X = 0.0f;
_navAngularVelocity.Y = 0.0f;
_navAngularVelocity.Z = 0.0f;
gate = true;
}
void NavUpdate(ScriptableRenderContext context, Camera[] cameras)
{
_unityPosition = ConventionTransforms.PositionUnityToNED(gameObject.transform.position);
_unityOrientation = UnityEngine.Quaternion.Euler(ConventionTransforms.EulerOrientationUnityToNED(gameObject.transform.rotation.eulerAngles));
// Set Navigation Position and orientation to the same value as the Unity position and orientation
// TODO: This should be done in a own function
// Publishes local coordinates
_navPosition.X = _unityPosition.x;
_navPosition.Y = _unityPosition.y;
//_navPosition.Z = _unityPosition.z;
// Publishes latitude and longitude
//_navPosition.X = _unityPosition.x * (float)Math.Atan2(1, R_M * Math.PI / 180f) + latOffset; // Latitude
//_navPosition.Y = _unityPosition.y * (float)Math.Atan2(1, R_N * Math.PI / 180f * Math.Cos(latOffset * Math.PI / 180f)) + lonOffset; // Longitude
_navPosition.Z = gameObject.transform.rotation.eulerAngles.y; // Heading
_navOrientation.X = _unityOrientation.x;
_navOrientation.Y = _unityOrientation.y;
_navOrientation.Z = _unityOrientation.z;
_navOrientation.W = _unityOrientation.w;
if (_rigidBody != null)
{
_unityLinearVelocity = ConventionTransforms.VelocityUnityToNED(_rigidBody.velocity);
_navLinearVelocity.X = _unityLinearVelocity.x;
_navLinearVelocity.Y = _unityLinearVelocity.y;
_navLinearVelocity.Z = _unityLinearVelocity.z;
_unityAngularVelocity = ConventionTransforms.AngularVelocityUnityToNED(_rigidBody.angularVelocity);
_navAngularVelocity.X = _unityAngularVelocity.x;
_navAngularVelocity.Y = _unityAngularVelocity.y;
_navAngularVelocity.Z = _unityAngularVelocity.z;
}
else
{
_navLinearVelocity.X = 0.0f;
_navLinearVelocity.Y = 0.0f;
_navLinearVelocity.Z = 0.0f;
_navAngularVelocity.X = 0.0f;
_navAngularVelocity.Y = 0.0f;
_navAngularVelocity.Z = 0.0f;
}
gate = true;
}
void NavUpdate(ScriptableRenderContext context, Camera[] cameras)
{
var pirenOffset = new Vector3(
-piren.position.x + gameObject.transform.position.x,
-piren.position.y + gameObject.transform.position.y,
-piren.position.z + gameObject.transform.position.z
);
//_unityPosition = ConventionTransforms.PositionUnityToNED(gameObject.transform.position);
_unityPosition = ConventionTransforms.PositionUnityToNED(pirenOffset);
_unityOrientation = UnityEngine.Quaternion.Euler(ConventionTransforms.EulerOrientationUnityToNED(gameObject.transform.rotation.eulerAngles));
// Set Navigation Position and orientation to the same value as the Unity position and orientation
// TODO: This should be done in a own function
_navPosition.X = _unityPosition.x;
_navPosition.Y = _unityPosition.y;
_navPosition.Z = _unityPosition.z;
_navOrientation.X = _unityOrientation.x;
_navOrientation.Y = _unityOrientation.y;
_navOrientation.Z = _unityOrientation.z;
_navOrientation.W = _unityOrientation.w;
if (_rigidBody != null)
{
_unityLinearVelocity = ConventionTransforms.VelocityUnityToNED(_rigidBody.velocity);
_navLinearVelocity.X = _unityLinearVelocity.x;
_navLinearVelocity.Y = _unityLinearVelocity.y;
_navLinearVelocity.Z = _unityLinearVelocity.z;
_unityAngularVelocity = ConventionTransforms.AngularVelocityUnityToNED(_rigidBody.angularVelocity);
_navAngularVelocity.X = _unityAngularVelocity.x;
_navAngularVelocity.Y = _unityAngularVelocity.y;
_navAngularVelocity.Z = _unityAngularVelocity.z;
}
else
{
_navLinearVelocity.X = 0.0f;
_navLinearVelocity.Y = 0.0f;
_navLinearVelocity.Z = 0.0f;
_navAngularVelocity.X = 0.0f;
_navAngularVelocity.Y = 0.0f;
_navAngularVelocity.Z = 0.0f;
}
gate = true;
}
public override async Task <ForceResponse> ApplyForce(ForceRequest request, ServerCallContext context)
{
_force.x = Mathf.Clamp(request.GeneralizedForce.X, -100f, 100f) * surge_Gain;
_force.y = Mathf.Clamp(request.GeneralizedForce.Y, -100f, 100f) * sway_Gain;
_force.z = 0; //request.GeneralizedForce.Z;
_torque.x = 0; //request.GeneralizedForce.K;
_torque.y = 0; //request.GeneralizedForce.M;
_torque.z = Mathf.Clamp(request.GeneralizedForce.N, -100f, 100f) * yaw_Gain;
// Create the event that triggers when the execution of the action is finished.
ManualResetEvent signalEvent = new ManualResetEvent(false);
ThreadManager.ExecuteOnMainThread(() =>
{
foreach (ForceController forceController in _forceControllers)
{
if (forceController.name == request.VesselId)
{
forceController.Force = ConventionTransforms.ForceNEDToUnity(_force);
forceController.Torque = ConventionTransforms.TorqueNEDToUnity(_torque);
}
}
signalEvent.Set();
});
// Wait for the event to be triggered from the action, signaling that the action is finished
// This is required becaue we are reading and depending on state from a resource running on the
// Unity main thread.
signalEvent.WaitOne();
signalEvent.Close();
return(await Task.FromResult(new ForceResponse
{
Success = true,
}));
}
