void LateUpdate()
{
//try {
if (zmq != null)
{
if (zmq.getNewestData(out output))
{
//Expect righthand, X front, Z up coordinate system
m_position = CoordinateTransformations.VectorToGameEngine(new Vector3(output.px, output.py, output.pz));
m_rotation = CoordinateTransformations.QuaternionToGameEngine(new Quaternion(output.qx, output.qy, output.qz, output.qw));
//update height if walking on ground
if (WalkOnGround)
{
//trace from new x,z coordinates and old height, with height = new height
m_position = FallToGround(m_position, m_position.y);
}
}
}
transform.position = m_position;
transform.rotation = m_rotation;
//trigger position update
TriggerPositionUpdate(m_position, m_rotation);
}
public static void GetHelioCentricState(DateTime epoch, Coordinates coordinates)
{
/*
* Possible use would be : get for a satellite several heliocentric states in different frames, though it is doubtful one would ever need this.
* Normally, you would expect to retrieve one set of Keplerian and Cartesian state with same frame
*/
if (coordinates.Body == null)
{
throw new ArgumentNullException();
}
Body body = coordinates.Body.Value;
KeplerianCoordinates keplerianCoordinates = coordinates.KeplerianCoordinates;
if (coordinates.CoordinatesNeeded.Keplerian)
{
GetHelioCentricKeplerianElements(body, epoch, keplerianCoordinates);
}
if (coordinates.CoordinatesNeeded.Cartesian)
{
KeplerianCoordinates dummyKeplerianCoordinates;
foreach (CartesianCoordinates cartesianCoordinates in coordinates.CartesianCoordinates)
{
cartesianCoordinates.Origin = Body.Sun;
if (keplerianCoordinates == null || keplerianCoordinates.CoordinateFrame != cartesianCoordinates.CoordinateFrame)//TESTME sending a nonnull KeplerianCoord with a null coordinate frame
{
dummyKeplerianCoordinates = new KeplerianCoordinates
{
CoordinateFrame = cartesianCoordinates.CoordinateFrame
};
GetHelioCentricKeplerianElements(body, epoch, dummyKeplerianCoordinates);
}
else
{
dummyKeplerianCoordinates = keplerianCoordinates;
}
CoordinateTransformations.ConvertKeplerianToCartesian(dummyKeplerianCoordinates, cartesianCoordinates);
}
}
else
{
throw new NotImplementedException();
}
}
public static DefaultState GetDefaultState(DateTime?epoch = null)
{
CartesianCoordinates cartesianCoordinates = new CartesianCoordinates
{
CoordinateFrame = Frame.EME2000,
Origin = Body.Earth,
};
Coordinates coordinates = new Coordinates
{
Body = Body.Earth,
CartesianCoordinates = new List <CartesianCoordinates>
{
cartesianCoordinates,
},
};
DefaultState defaultState = new DefaultState
{
CoordinateType = CoordinateType.Cartesian,
Coordinates = coordinates,
};
if (epoch == null)
{
return(defaultState); //FIXME
}
else
{
defaultState.Epoch = epoch;
DefaultState earthDefaultState = Earth.GetDefaultState(epoch);
CartesianCoordinates earthCartesian = new CartesianCoordinates
{
Depth = CartesianDepth.Velocity,
};
CoordinateTransformations.ConvertKeplerianToCartesian(earthDefaultState.Coordinates.KeplerianCoordinates, earthCartesian);
earthCartesian.Negative();
cartesianCoordinates = earthCartesian;
cartesianCoordinates.Origin = Body.Earth;
defaultState.Coordinates.CartesianCoordinates = new List <CartesianCoordinates>
{
cartesianCoordinates
};
return(defaultState);
}
}
public void positionUpdate(Vector3 pos, Quaternion ori)
{
miVRlink.Position6DofMessage msg = new miVRlink.Position6DofMessage();
pos = CoordinateTransformations.GameEngineToVector(pos);
ori = CoordinateTransformations.GameEngineToQuaternion(ori);
msg.timestamp_system = Time.time;
msg.timestamp_device = Time.time;
msg.px = pos.x;
msg.py = pos.y;
msg.pz = pos.z;
msg.qw = ori.w;
msg.qx = ori.x;
msg.qy = ori.y;
msg.qz = ori.z;
zmq.sendData(msg);
}
private void SendRobotTargetPose(TimeSpan currentTime, Quaternion currentRotation, Vector3 currentPosition)
{
int[] structuredTime = Timer.GetSecondsNanosecondsStructure(currentTime);
PoseStamped message = new PoseStamped();
message.header.frame_id = Config.HololensWorldFrame;
message.header.seq = robotTargetPoseFrameIdx++;
message.header.stamp.secs = structuredTime[0];
message.header.stamp.nsecs = structuredTime[1];
CoordinateTransformations.ConvertPoseUnity2ROS(ref currentPosition, ref currentRotation);
message.pose.orientation.x = Quaternion.identity.x;
message.pose.orientation.y = Quaternion.identity.y;
message.pose.orientation.z = Quaternion.identity.z;
message.pose.orientation.w = Quaternion.identity.w;
message.pose.position.x = currentPosition.x;
message.pose.position.y = currentPosition.y;
message.pose.position.z = currentPosition.z;
robotTargetPosePublisher.Publish(message);
System.Diagnostics.Debug.WriteLine("ROBOT TARGET POSE at time: " + ((double)structuredTime[0] + ((double)structuredTime[1]) / 1e9).ToString() +
" --- (" + currentPosition.x + ", " + currentPosition.y + ", " + currentPosition.z + ")");
}
private void SendPose(TimeSpan currentTime, Quaternion currentRotation, Vector3 currentPosition)
{
int[] structuredTime = Timer.GetSecondsNanosecondsStructure(currentTime);
RosSharp.RosBridgeClient.Messages.Geometry.PoseStamped message = new RosSharp.RosBridgeClient.Messages.Geometry.PoseStamped();
message.header.frame_id = Config.HololensWorldFrame;
message.header.seq = frameIdx++;
message.header.stamp.secs = structuredTime[0];
message.header.stamp.nsecs = structuredTime[1];
CoordinateTransformations.ConvertPoseUnity2ROS(ref currentPosition, ref currentRotation);
message.pose.orientation.x = currentRotation.x;
message.pose.orientation.y = currentRotation.y;
message.pose.orientation.z = currentRotation.z;
message.pose.orientation.w = currentRotation.w;
message.pose.position.x = currentPosition.x;
message.pose.position.y = currentPosition.y;
message.pose.position.z = currentPosition.z;
publisher.Publish(message);
System.Diagnostics.Debug.WriteLine("POSE at time: " + ((double)structuredTime[0] + ((double)structuredTime[1]) / 1e9).ToString() +
" --- (" + currentPosition.x + ", " + currentPosition.y + ", " + currentPosition.z + ")");
}
void doUpdate()
{
//try {
if (zmq != null)
{
if (zmq.getNewestData(out output))
{
//Expect righthand, X front, Z up coordinate system
pos = CoordinateTransformations.VectorToGameEngine(new Vector3(output.px, output.py, output.pz));
ori = CoordinateTransformations.QuaternionToGameEngine(new Quaternion(output.qx, output.qy, output.qz, output.qw));
m_rotation = ori;
//update height if walking on ground
if (WalkOnGround)
{
//trace from new x,z coordinates and old height, with height = new height
m_position = FallToGround(new Vector3(pos.x, m_position.y, pos.z), pos.y);
}
else
{
m_position = pos;
}
}
Invoke("doUpdate", updateTime);
}
else
{
Invoke("doUpdate", 1f);
}
//trigger position update
TriggerPositionUpdate(m_position, m_rotation);
}
public static void GetState(State state)
{
if (state == null)
{
throw new ArgumentNullException();
}
state.CheckValidity();
Body body = state.Body.Value;
DateTime epoch = state.Epoch;
DefaultState bodyDefaultState = CelestialBodies.GetDefaultState(body, epoch);
defaultStates.Add(body, bodyDefaultState);
foreach (Coordinates mainCoordinates in state.CoordinatesSet)
{
if (mainCoordinates.CoordinatesNeeded == null || mainCoordinates.Body == null)
{
throw new ArgumentNullException();
}
Body origin = mainCoordinates.Body.Value;
if (origin == body)
{
throw new InvalidOperationException();
}
DefaultState originDefaultState = new DefaultState();
if (bodyDefaultState.Coordinates.Body != origin)
{
originDefaultState = CelestialBodies.GetDefaultState(origin);
defaultStates.Add(origin, originDefaultState);
}
CoordinatesNeeded coordinatesNeeded = mainCoordinates.CoordinatesNeeded;
if (coordinatesNeeded.Keplerian)
{
//Earth-Sun, Sun-Earth
if (bodyDefaultState.Coordinates.Body == origin)
{
if (bodyDefaultState.CoordinateType == CoordinateType.Keplerian)
{
CoordinateTransformations.ConvertKeplerianFrame(bodyDefaultState.Coordinates.KeplerianCoordinates, mainCoordinates.KeplerianCoordinates);
}
else if (bodyDefaultState.CoordinateType == CoordinateType.Cartesian)
{
CoordinateTransformations.ConvertCartesianToKeplerian(bodyDefaultState.Coordinates.CartesianCoordinates.First(), mainCoordinates.KeplerianCoordinates);
}
else
{
throw new NotImplementedException();
}
}
// Sun-Mars , Sun default earth, Mars default sun
else if (originDefaultState.Coordinates.Body == body)
{
CartesianCoordinates dummyCartesianCoordinates = new CartesianCoordinates
{
CoordinateFrame = mainCoordinates.KeplerianCoordinates.CoordinateFrame,
Depth = CartesianDepth.Velocity,
};
if (originDefaultState.CoordinateType == CoordinateType.Keplerian)
{
CartesianCoordinates cartesianCoordinates1 = new CartesianCoordinates
{
Depth = CartesianDepth.Velocity,
};
CoordinateTransformations.ConvertKeplerianToCartesian(originDefaultState.Coordinates.KeplerianCoordinates, cartesianCoordinates1);
CoordinateTransformations.ConvertCartesianFrame(cartesianCoordinates1, dummyCartesianCoordinates);
}
else if (originDefaultState.CoordinateType == CoordinateType.Cartesian)
{
CoordinateTransformations.ConvertCartesianFrame(originDefaultState.Coordinates.CartesianCoordinates.First(), dummyCartesianCoordinates);
}
dummyCartesianCoordinates.Negative();
CoordinateTransformations.ConvertCartesianToKeplerian(dummyCartesianCoordinates, mainCoordinates.KeplerianCoordinates);
}
//Moon-Sun
//Sun-Moon
}
if (coordinatesNeeded.Cartesian)
{
foreach (CartesianCoordinates cartesianCoordinates in mainCoordinates.CartesianCoordinates)
{
if (cartesianCoordinates == null || cartesianCoordinates.CoordinateFrame == null)
{
throw new ArgumentNullException();
}
cartesianCoordinates.Origin = origin;
if (origin != body)
{
CartesianCoordinates dummyCartesianCoordinates = new CartesianCoordinates
{
Depth = cartesianCoordinates.Depth,
CoordinateFrame = cartesianCoordinates.CoordinateFrame,
};
Coordinates dummyCoordinates = new Coordinates
{
Body = origin,
CoordinatesNeeded = new CoordinatesNeeded
{
Cartesian = true,
},
KeplerianCoordinates = mainCoordinates.KeplerianCoordinates,
CartesianCoordinates = new List <CartesianCoordinates>
{
dummyCartesianCoordinates,
}
};
GetHelioCentricState(state.Epoch, dummyCoordinates);
dummyCartesianCoordinates.Negative(); //BETTERME
cartesianCoordinates.Position = dummyCartesianCoordinates.Position;
if (cartesianCoordinates.Depth >= CartesianDepth.Velocity)
{
cartesianCoordinates.Velocity = dummyCartesianCoordinates.Velocity;
}
}
else
{
cartesianCoordinates.Position = Vector <double> .Build.Dense(3);
if (cartesianCoordinates.Depth >= CartesianDepth.Velocity)
{
cartesianCoordinates.Velocity = Vector <double> .Build.Dense(3);
}
}
}
}
}
}
