protected override void Draw(GameTime gameTime)
{
// Clear the screen
//GraphicsDevice.Clear(Color.CornflowerBlue);
GraphicsDevice.Clear(new Color(41, 51, 86));
//GraphicsDevice.Clear(Color.White);
for (int eyeIndex = 0; eyeIndex < 2; eyeIndex++)
{
SETUP_eyeRender(eyeIndex);
Draw_SkySurface(gameTime);
Draw_Model(gameTime);
Draw_BaslerCamera(gameTime);
Draw_RoboticArm();
//Draw_Font((int)eye);
//Draw_SoundGraphicalEffects();
}
BeginDraw_Font();
for (int eyeIndex = 0; eyeIndex < 2; eyeIndex++) // need second loop as font drawing uses stencil buffer
{
SETUP_eyeRender(eyeIndex); // ask for the values from hmd again as they may have changed from the point where other models were drown
Draw_Font(eyeIndex);
}
EndDraw_Font();
DataAcquisition();
HUD.AppendLine(string.Format("hmd latency = {0,7:0.00000000}", hmd.GetMeasuredLatency()));
}
public void Update_RoboticArmDrawnPosture()
{
ra.UPDATE_matrices(eyeProjection, eyeView, eyeWorld);
List <C_Value> yawPitchRoll;
//if (config.roboticArmPostureOnCameraCapture == false)
//{
yawPitchRoll = GetAngleValuesFromMotors_Reference(ra.angleType);
//}
//else
//{
// yawPitchRoll = config.cameraControl.YawPitchRollOnCapture;
//}
ra.UPDATE_PartRotationAndWorldMatrix(
(float)yawPitchRoll[0].Rad_FromDefault,
(float)yawPitchRoll[1].Rad_FromDefault,
(float)yawPitchRoll[2].Rad_FromDefault
);
HUD.AppendLine(string.Format("READ YawPitchRoll[deg] [{0,7:0.00}|{1,7:0.00}|{2,7:0.00}]",
(float)yawPitchRoll[0].Dec_FromDefault,
(float)yawPitchRoll[1].Dec_FromDefault,
(float)yawPitchRoll[2].Dec_FromDefault
));
}
void Update_Input()
{
keyboardState = keyboardManager.GetState();
mouseState = mouseManager.GetState();
// if Esc is pressed - quit program
if (keyboardState.IsKeyPressed(Keys.Escape))
{
Exit();
return;
}
if (keyboardState.IsKeyPressed(Keys.K))
{
tiles[3].PlayDelegate();
}
if (keyboardState.IsKeyPressed(Keys.J))
{
tiles[3].StopDelegate();
}
if (keyboardState.IsKeyPressed(Keys.O))
{
tiles[4].PlayDelegate();
}
if (keyboardState.IsKeyPressed(Keys.P))
{
tiles[4].StopDelegate();
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
int mouseWheelOffset = mouseState.WheelDelta - lastWheelDelta;
if (mouseWheelOffset != 0)
{
int multiply = 10;
if (keyboardState.IsKeyDown(Keys.Control))
{
multiply = 1;
}
if (keyboardState.IsKeyDown(Keys.Shift))
{
multiply = 100;
}
config.cameraControl.ExposureTime += multiply * mouseWheelOffset;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// info tiles
if (keyboardState.IsKeyPressed(Keys.F1))
{
config.hud.helpMenu ^= true;
hmd.EnableHSWDisplaySDKRender(true);
hmd.DismissHSWDisplay(); // not functional
}
if (keyboardState.IsKeyPressed(Keys.F2))
{
config.hud.toolStrip ^= true;
}
if (keyboardState.IsKeyPressed(Keys.F3))
{
config.hud.timeStrip ^= true;
}
if (keyboardState.IsKeyPressed(Keys.F5))
{
ra.angleType = e_valueType.wantedValue;
}
if (keyboardState.IsKeyPressed(Keys.F6))
{
ra.angleType = e_valueType.sentValue;
}
if (keyboardState.IsKeyPressed(Keys.F7))
{
ra.angleType = e_valueType.seenValue;
}
if (keyboardState.IsKeyPressed(Keys.F8))
{
config.roboticArmPostureOnCameraCapture ^= true;
}
if (keyboardState.IsKeyPressed(Keys.M) && keyboardState.IsKeyDown(Keys.Control))
{
//config.READ_dataFromMotors ^= true; // toggle
config.player.ResetPositionAndBodyYaw();
config.WRITE_dataToMotors ^= true;
}
else if (keyboardState.IsKeyPressed(Keys.M) && keyboardState.IsKeyDown(Keys.Shift))
{
//config.READ_dataFromMotors ^= true; // toggle
config.READ_dataFromMotors ^= true;
}
else if (keyboardState.IsKeyPressed(Keys.M))
{
config.motorSpeedControl ^= true;
}
if (keyboardState.IsKeyPressed(Keys.R) && keyboardState.IsKeyDown(Keys.Control))
{
config.player.ResetPosition();
}
else if (keyboardState.IsKeyPressed(Keys.R) && keyboardState.IsKeyDown(Keys.Shift))
{
config.player.ResetBodyYaw();
}
else if (keyboardState.IsKeyPressed(Keys.R))
{
config.player.ResetPositionAndBodyYaw();
}
if (keyboardState.IsKeyPressed(Keys.F))
{
config.draw.RoboticArm ^= true;
ra.draw = config.draw.RoboticArm;
}
if (keyboardState.IsKeyPressed(Keys.C) && keyboardState.IsKeyDown(Keys.Shift))
{
config.ReadCameraStream ^= true;
if (config.ReadCameraStream)
{
START_streaming();
}
else
{
STOP_streaming();
}
}
if (keyboardState.IsKeyPressed(Keys.U))
{
if (textureConversionAlgorithm == e_textureConversionAlgorithm.unsafeConversion_pointerForLoop)
{
// last
textureConversionAlgorithm = 0;
}
else
{
textureConversionAlgorithm++;
}
}
if (keyboardState.IsKeyPressed(Keys.D1) && keyboardState.IsKeyDown(Keys.Shift))
{
config.player.PositionLock = e_positionLock.cameraSensor;
}
if (keyboardState.IsKeyPressed(Keys.D2) && keyboardState.IsKeyDown(Keys.Shift))
{
config.player.PositionLock = e_positionLock.desk;
}
if (keyboardState.IsKeyPressed(Keys.D3) && keyboardState.IsKeyDown(Keys.Shift))
{
config.player.PositionLock = e_positionLock.overDesk;
}
if (keyboardState.IsKeyPressed(Keys.Tab))
{
config.player.PositionLockActive ^= true;
}
if (keyboardState.IsKeyPressed(Keys.D1))
{
config.cameraArtificialDelay = false;
lock (queuePixelData_locker)
{
que.Clear();
}
}
if (keyboardState.IsKeyPressed(Keys.D2))
{
config.cameraArtificialDelay = true;
lock (queuePixelData_locker)
{
que.Clear();
}
config.cameraFrameQueueLength = config.cameraFrameQueueLengthList[0];
}
if (keyboardState.IsKeyPressed(Keys.D3))
{
config.cameraArtificialDelay = true;
lock (queuePixelData_locker)
{
que.Clear();
}
config.cameraFrameQueueLength = config.cameraFrameQueueLengthList[1];
}
if (keyboardState.IsKeyPressed(Keys.D4))
{
config.cameraArtificialDelay = true; lock (queuePixelData_locker)
{
que.Clear();
}
config.cameraFrameQueueLength = config.cameraFrameQueueLengthList[2];
}
if (keyboardState.IsKeyPressed(Keys.D5))
{
config.cameraArtificialDelay = true;
lock (queuePixelData_locker)
{
que.Clear();
}
config.cameraFrameQueueLength = config.cameraFrameQueueLengthList[3];
}
if (keyboardState.IsKeyPressed(Keys.D6))
{
config.cameraArtificialDelay = true;
lock (queuePixelData_locker)
{
que.Clear();
}
config.cameraFrameQueueLength = config.cameraFrameQueueLengthList[4];
}
if (keyboardState.IsKeyPressed(Keys.X))
{
measurementStart = DateTime.Now;
}
HUD.AppendLine(string.Format(
"W{0}|A{1}|S{2}",
keyboardState.IsKeyDown(Keys.W),
keyboardState.IsKeyDown(Keys.A),
keyboardState.IsKeyDown(Keys.S)
));
config.player.FrameTime = gameTime.ElapsedGameTime.Milliseconds;
config.player.MoveForward(keyboardState.IsKeyDown(Keys.W));
config.player.MoveSideStep(keyboardState.IsKeyDown(Keys.A), false);
config.player.MoveBackward(keyboardState.IsKeyDown(Keys.S));
config.player.MoveSideStep(keyboardState.IsKeyDown(Keys.D), true);
config.player.MoveUpward(keyboardState.IsKeyDown(Keys.E));
config.player.MoveDownward(keyboardState.IsKeyDown(Keys.Q));
config.player.SetupSpeed(keyboardState.IsKeyDown(Keys.Shift), keyboardState.IsKeyDown(Keys.Control));
if (keyboardState.IsKeyPressed(Keys.J) && keyboardState.IsKeyDown(Keys.Control))
{
config.draw.SkySurface ^= true;
}
List <Keys> keys = new List <Keys>();
keyboardState.GetDownKeys(keys);
//foreach (var key in keys)
// sb.AppendFormat("Key: {0}, Code: {1}\n", key, (int)key);
// numer keys (NOT numpad ones) have name like D0, D1, etc...
// associate available modes each with its key
//for (int i = 0; i < availableModes.Count; i++)
//{
// var key = (Keys)Enum.Parse(typeof(Keys), "D" + i);
// if (keyboardState.IsKeyPressed(key))
// {
// ApplyMode(availableModes[i]);
// return;
// }
//}
lastWheelDelta = mouseState.WheelDelta;
}
public void Update_helpText()
{
HUD.AppendLine("[!]alt|[^]ctrl|[+]shift|[#]super");
HUD.AppendLine(string.Format("Control: [^M]otor={0}|", config.WRITE_dataToMotors));
HUD.AppendLine(string.Format("MotorSpeedControl: [M]{0}", config.motorSpeedControl));
HUD.AppendLine(string.Format("Read: [+M]otor={0}|[+C]amera={1}", config.READ_dataFromMotors, config.ReadCameraStream));
HUD.AppendLine(string.Format("PositionLock: [Tab]={0}|[numbers]={1}:{2}",
config.player.PositionLockActive,
(int)config.player.PositionLock, config.player.PositionLock
));
HUD.AppendLine(string.Format("RoboticArmUpdatingFromAngle: [F5,F6,F7]={0}",
ra.angleType
));
HUD.AppendLine(string.Format("roboticArmPostureOnCameraCapture: [F8]={0}",
config.roboticArmPostureOnCameraCapture
));
HUD.AppendLine("");
HUD.AppendLine("CameraArtificialDelay = " + config.cameraArtificialDelay.ToString());
if (config.cameraArtificialDelay == true)
{
HUD.AppendLine("actual queue frame length = " + queuePixelData.Count().ToString());
HUD.AppendLine("setuped cameraFrameQueueLength = " + config.cameraFrameQueueLength.ToString());
HUD.AppendLine(string.Format("Actual artificial delay = {0,9:F4} [ms]", qAct.TotalMilliseconds));
}
HUD.AppendLine(string.Format("Status: [Camera={0}][Motors={1}][SPI={2}]",
C_State.baslerCam,
C_State.mot,
C_State.Spi
));
// may be in config constructor
string name = "No camera connected - please connect the camera and restart telepresence!";
if (C_State.FURTHER(e_stateBaslerCam.initializing))
{
name = config.cameraControl.camera.CameraInfo[Basler.Pylon.CameraInfoKey.DeviceType] + " " +
config.cameraControl.camera.CameraInfo[Basler.Pylon.CameraInfoKey.ModelName]
;
HUD.AppendLine("Camera: " + name);
HUD.AppendLine(string.Format(
"Camera: [Connected={0}], [Open={1}], [Grabbing={2}]",
config.cameraControl.camera.IsConnected,
config.cameraControl.camera.IsOpen,
config.cameraControl.camera.StreamGrabber.IsGrabbing
));
HUD.AppendLine(string.Format("[U]=[textureConversionAlgorithm={0}]", textureConversionAlgorithm));
HUD.AppendLine(string.Format("[+][-]=[ExposureTime={0:F3}ms]",
config.cameraControl.ExposureTime / 1000));
}
HUD.AppendLine("");
Vector3 pos;
Vector3 rot;
pos = config.player.scout.Position;
HUD.AppendLine(string.Format("scout [XYZ]: [{0,7:0.00}|{1,7:0.00}|{2,7:0.00}]",
pos[0], pos[1], pos[2]
));
rot = config.player.body.YawPitchRoll;
HUD.AppendLine(string.Format("body YawPitchRoll [Yaw|Pitch|Roll]: [{0,7:0.00}|{1,7:0.00}|{2,7:0.00}]",
rot[0], rot[1], rot[2]
));
pos = config.player.hmd.Position;
HUD.AppendLine(string.Format("hmd [XYZ]: [{0,7:0.00}|{1,7:0.00}|{2,7:0.00}]",
pos[0], pos[1], pos[2]
));
rot = config.player.hmd.YawPitchRoll;
HUD.AppendLine(string.Format("hmd [Yaw|Pitch|Roll]: [{0,7:0.00}|{1,7:0.00}|{2,7:0.00}]",
rot[0], rot[1], rot[2]
));
pos = config.player.Position;
HUD.AppendLine(string.Format("Player [XYZ]: [{0,7:0.00}|{1,7:0.00}|{2,7:0.00}]",
pos[0], pos[1], pos[2]
));
rot = config.player.YawPitchRoll;
HUD.AppendLine(string.Format("Player [Yaw|Pitch|Roll]: [{0,7:0.00}|{1,7:0.00}|{2,7:0.00}]",
rot[0], rot[1], rot[2]
));
}