public unsafe void writeInput(NetworkStream stream)
{
MJP.TPerturb perturb;
MJP.GetPerturb(&perturb);
stream.Write(BitConverter.GetBytes(lastkey), 0, 4);
stream.Write(BitConverter.GetBytes(perturb.select), 0, 4);
stream.Write(BitConverter.GetBytes(perturb.active), 0, 4);
stream.Write(BitConverter.GetBytes(perturb.refpos[0]), 0, 4);
stream.Write(BitConverter.GetBytes(perturb.refpos[1]), 0, 4);
stream.Write(BitConverter.GetBytes(perturb.refpos[2]), 0, 4);
stream.Write(BitConverter.GetBytes(perturb.refquat[0]), 0, 4);
stream.Write(BitConverter.GetBytes(perturb.refquat[1]), 0, 4);
stream.Write(BitConverter.GetBytes(perturb.refquat[2]), 0, 4);
stream.Write(BitConverter.GetBytes(perturb.refquat[3]), 0, 4);
lastkey = 0;
}
// per-frame update
unsafe void Update()
{
//New Stuff
// Debug.Log(maze_agent_pos.position.ToString());
// mouse interaction
ProcessMouse();
UpdateModel();
// check conection each 0.1 sec
if (lastcheck + 0.1f < Time.time)
{
// broken connection: clear
if (!CheckConnection())
{
client = null;
stream = null;
}
lastcheck = Time.time;
}
// not connected: accept connection if pending
if (client == null || !client.Connected)
{
if (listener.Pending())
{
// make connection
client = listener.AcceptTcpClient();
stream = client.GetStream();
// send 20 bytes: nqpos, nmocap, ncamera, width, height
stream.Write(BitConverter.GetBytes(nqpos), 0, 4);
stream.Write(BitConverter.GetBytes(nmocap), 0, 4);
stream.Write(BitConverter.GetBytes(ncamera), 0, 4);
stream.Write(BitConverter.GetBytes(offwidth), 0, 4);
stream.Write(BitConverter.GetBytes(offheight), 0, 4);
}
}
// data available: handle communication
while (client != null && client.Connected && stream != null && stream.DataAvailable)
{
// get command
ReadAll(4);
int cmd = BitConverter.ToInt32(buffer, 0);
// process command
switch ((Command)cmd)
{
// GetInput: send lastkey, select, active, refpos[3], refquat[4]
case Command.GetInput:
MJP.TPerturb perturb;
MJP.GetPerturb(&perturb);
stream.Write(BitConverter.GetBytes(lastkey), 0, 4);
stream.Write(BitConverter.GetBytes(perturb.select), 0, 4);
stream.Write(BitConverter.GetBytes(perturb.active), 0, 4);
stream.Write(BitConverter.GetBytes(perturb.refpos[0]), 0, 4);
stream.Write(BitConverter.GetBytes(perturb.refpos[1]), 0, 4);
stream.Write(BitConverter.GetBytes(perturb.refpos[2]), 0, 4);
stream.Write(BitConverter.GetBytes(perturb.refquat[0]), 0, 4);
stream.Write(BitConverter.GetBytes(perturb.refquat[1]), 0, 4);
stream.Write(BitConverter.GetBytes(perturb.refquat[2]), 0, 4);
stream.Write(BitConverter.GetBytes(perturb.refquat[3]), 0, 4);
lastkey = 0;
break;
// GetImage: send 3*width*height bytes
case Command.GetImage:
RenderToTexture();
stream.Write(offtex.GetRawTextureData(), 0, 3 * offwidth * offheight);
break;
// SaveSnapshot: no data exchange
case Command.SaveSnapshot:
RenderToTexture();
byte[] bytes = offtex.EncodeToPNG();
File.WriteAllBytes(Application.streamingAssetsPath + "/../../" + "img_" +
snapshots + ".png", bytes);
snapshots++;
break;
// SaveVideoframe: no data exchange
case Command.SaveVideoframe:
if (videofile == null)
{
videofile = new FileStream(Application.streamingAssetsPath + "/../../" + "video.raw",
FileMode.Create, FileAccess.Write);
}
RenderToTexture();
videofile.Write(offtex.GetRawTextureData(), 0, 3 * offwidth * offheight);
break;
// SetCamera: receive camera index
case Command.SetCamera:
ReadAll(4);
camindex = BitConverter.ToInt32(buffer, 0);
camindex = Math.Max(-1, Math.Min(ncamera - 1, camindex));
break;
// SetQpos: receive qpos vector
case Command.SetQpos:
if (nqpos > 0)
{
ReadAll(4 * nqpos);
fixed(byte *qpos = buffer)
{
MJP.SetQpos((float *)qpos);
}
MJP.Kinematics();
UpdateModel();
}
break;
// SetMocap: receive mocap_pos and mocap_quat vectors
case Command.SetMocap:
if (nmocap > 0)
{
ReadAll(28 * nmocap);
fixed(byte *pos = buffer, quat = &buffer[12 * nmocap])
{
MJP.SetMocap((float *)pos, (float *)quat);
}
MJP.Kinematics();
UpdateModel();
}
break;
// GetTarget: send maze_agent_pos
case Command.GetTarget:
float target_x = -1f * maze_agent_pos.position.x;
float target_y = -1f * maze_agent_pos.position.z;
if (move_maze == true)
{
break;
}
float dist = Vector3.Distance(humanoid_pos.position, maze_agent_pos.position);
// Debug.Log(dist);
if (dist < 1.0f)
{
move_maze = true;
Debug.Log(dist);
}
stream.Write(BitConverter.GetBytes(target_x), 0, 4);
stream.Write(BitConverter.GetBytes(target_y), 0, 4);
break;
case Command.SetInit:
ag_reset = true;
break;
}
}
}
// per-frame mouse input; called from Update
unsafe void ProcessMouse()
{
// get modifiers
bool alt = Input.GetKey(KeyCode.LeftAlt) || Input.GetKey(KeyCode.RightAlt);
bool shift = Input.GetKey(KeyCode.LeftShift) || Input.GetKey(KeyCode.RightShift);
bool control = Input.GetKey(KeyCode.LeftControl) || Input.GetKey(KeyCode.RightControl);
// get button pressed, swap left-right on alt
int buttonpressed = 0;
if (Input.GetMouseButton(0)) // left
{
buttonpressed = (alt ? 2 : 1);
}
if (Input.GetMouseButton(1)) // right
{
buttonpressed = (alt ? 1 : 2);
}
if (Input.GetMouseButton(2)) // middle
{
buttonpressed = 3;
}
// get button click, swap left-right on alt
int buttonclick = 0;
if (Input.GetMouseButtonDown(0)) // left
{
buttonclick = (alt ? 2 : 1);
}
if (Input.GetMouseButtonDown(1)) // right
{
buttonclick = (alt ? 1 : 2);
}
if (Input.GetMouseButtonDown(2)) // middle
{
buttonclick = 3;
}
// click
if (buttonclick > 0)
{
// set perturbation state
int newstate = 0;
if (control)
{
// determine new perturbation state
if (buttonclick == 1)
{
newstate = 2; // rotate
}
else if (buttonclick == 2)
{
newstate = 1; // move
}
// get old perturbation state
MJP.TPerturb current;
MJP.GetPerturb(¤t);
// syncronize if starting perturbation now
if (newstate > 0 && current.active == 0)
{
MJP.PerturbSynchronize();
}
}
MJP.PerturbActive(newstate);
// process double-click
if (buttonclick == lastbutton && Time.fixedUnscaledTime - lasttime < 0.25)
{
// relative screen position and aspect ratio
float relx = Input.mousePosition.x / Screen.width;
float rely = Input.mousePosition.y / Screen.height;
float aspect = (float)Screen.width / (float)Screen.height;
// left: select body
if (buttonclick == 1)
{
MJP.PerturbSelect(relx, rely, aspect);
}
// right: set lookat
else if (buttonclick == 2)
{
MJP.CameraLookAt(relx, rely, aspect);
}
}
// save mouse state
lastx = Input.mousePosition.x;
lasty = Input.mousePosition.y;
lasttime = Time.fixedUnscaledTime;
lastbutton = buttonclick;
}
// left or right drag: manipulate camera or perturb
if (buttonpressed == 1 || buttonpressed == 2)
{
// compute relative displacement and modifier
float reldx = (Input.mousePosition.x - lastx) / Screen.height;
float reldy = (Input.mousePosition.y - lasty) / Screen.height;
int modifier = (shift ? 1 : 0);
// perturb
if (control)
{
if (buttonpressed == 1)
{
MJP.PerturbRotate(reldx, -reldy, modifier);
}
else
{
MJP.PerturbMove(reldx, -reldy, modifier);
}
}
// camera
else
{
if (buttonpressed == 1)
{
MJP.CameraRotate(reldx, -reldy);
}
else
{
MJP.CameraMove(reldx, -reldy, modifier);
}
}
}
// middle drag: zoom camera
if (buttonpressed == 3)
{
float reldy = (Input.mousePosition.y - lasty) / Screen.height;
MJP.CameraZoom(-reldy);
}
// scroll: zoom camera
if (Input.mouseScrollDelta.y != 0)
{
MJP.CameraZoom(-0.05f * Input.mouseScrollDelta.y);
}
// save position
lastx = Input.mousePosition.x;
lasty = Input.mousePosition.y;
// release left or right: stop perturb
if (Input.GetMouseButtonUp(0) || Input.GetMouseButtonUp(1))
{
MJP.PerturbActive(0);
}
}
// per-frame update
unsafe void Update()
{
// mouse interaction
ProcessMouse();
UpdateModel();
// check conection each 0.1 sec
if (lastcheck + 0.1f < Time.time)
{
// broken connection: clear
if (!CheckConnection())
{
client = null;
stream = null;
counter.enabled = false;
title.enabled = true;
subtitle.enabled = true;
prompt.SetActive(false);
}
else
{
if (!prompt_mode)
{
counter.enabled = true;
}
title.enabled = false;
subtitle.enabled = false;
}
lastcheck = Time.time;
}
OVRInput.Update();
if (!prompt_mode)
{
Vector3 controllerpos = OVRInput.GetLocalControllerPosition(OVRInput.Controller.RTouch);
Quaternion controllerquat = OVRInput.GetLocalControllerRotation(OVRInput.Controller.RTouch);
posbuf[0] = Mathf.Clamp(0.8f + -1 * controllerpos.x * 1.5f, 0.4f, 1.2f);
posbuf[1] = Mathf.Clamp(1f + -1 * controllerpos.z * 1.5f, 0.4f, 0.9f);
posbuf[2] = Mathf.Clamp(3.2f + controllerpos.y * 4f, 0.0f, 0.8f);
quatbuf[0] = controllerquat.x;
quatbuf[1] = controllerquat.y;
quatbuf[2] = controllerquat.z;
quatbuf[3] = controllerquat.w;
grip = 1f - 2f * OVRInput.Get(OVRInput.Axis1D.PrimaryIndexTrigger, OVRInput.Controller.RTouch);
Vector2 lstick = OVRInput.Get(OVRInput.Axis2D.PrimaryThumbstick, OVRInput.Controller.LTouch);
camera_pos.position = new Vector3(camera_pos.position.x + 0.03f * lstick.x, camera_pos.position.y, camera_pos.position.z + 0.03f * lstick.y);
if (OVRInput.Get(OVRInput.Button.PrimaryThumbstick, OVRInput.Controller.LTouch))
{
camera_pos.position = camera_init_pos;
}
//controllerpos = new Vector3(posbuf[0], posbuf[1], posbuf[2]);
//controllerpos = new Vector3(-10*0.8f, 10 * 0.4f,-10 *0.65f);
controllerpos = new Vector3(-10 * posbuf[0], 10 * posbuf[2], -10 * posbuf[1]);
controller_tracker.position = controllerpos;
//Debug.Log(mocap_pos.position - controllerpos);
float delta_x = -1 * Mathf.Sign(controllerpos.x - mocap_pos.position.x) * Mathf.Min(Mathf.Abs(controllerpos.x - mocap_pos.position.x), 2.0f) / 2.0f;
float delta_y = -1 * Mathf.Sign(controllerpos.z - mocap_pos.position.z) * Mathf.Min(Mathf.Abs(controllerpos.z - mocap_pos.position.z), 1.25f) / 1.25f;
float delta_z = Mathf.Sign(controllerpos.y - mocap_pos.position.y) * Mathf.Min(Mathf.Abs(controllerpos.y - mocap_pos.position.y), 2.0f) / 2.0f;
posbuf[0] = delta_x;
posbuf[1] = delta_y;
posbuf[2] = delta_z;
//Debug.Log(delta_x);
counter.text = steps.ToString();
}
else
{
if (OVRInput.Get(OVRInput.Button.One, OVRInput.Controller.RTouch))
{
stream.Write(BitConverter.GetBytes(1), 0, 4);
prompt_mode = false;
counter.enabled = true;
prompt.SetActive(false);
}
else if (OVRInput.Get(OVRInput.Button.Two, OVRInput.Controller.RTouch))
{
stream.Write(BitConverter.GetBytes(0), 0, 4);
prompt_mode = false;
counter.enabled = true;
prompt.SetActive(false);
}
else if (OVRInput.Get(OVRInput.Button.One, OVRInput.Controller.LTouch))
{
stream.Write(BitConverter.GetBytes(-1), 0, 4);
Application.Quit();
}
}
// not connected: accept connection if pending
if (client == null || !client.Connected)
{
if (listener.Pending())
{
// make connection
client = listener.AcceptTcpClient();
stream = client.GetStream();
// send 20 bytes: nqpos, nmocap, ncamera, width, height
stream.Write(BitConverter.GetBytes(nqpos), 0, 4);
stream.Write(BitConverter.GetBytes(nmocap), 0, 4);
stream.Write(BitConverter.GetBytes(ncamera), 0, 4);
stream.Write(BitConverter.GetBytes(offwidth), 0, 4);
stream.Write(BitConverter.GetBytes(offheight), 0, 4);
}
}
// data available: handle communication
while (client != null && client.Connected && stream != null && stream.DataAvailable)
{
// get command
ReadAll(4);
int cmd = BitConverter.ToInt32(buffer, 0);
// process command
switch ((Command)cmd)
{
// GetInput: send lastkey, select, active, refpos[3], refquat[4]
case Command.GetInput:
MJP.TPerturb perturb;
MJP.GetPerturb(&perturb);
stream.Write(BitConverter.GetBytes(lastkey), 0, 4);
stream.Write(BitConverter.GetBytes(perturb.select), 0, 4);
stream.Write(BitConverter.GetBytes(perturb.active), 0, 4);
stream.Write(BitConverter.GetBytes(perturb.refpos[0]), 0, 4);
stream.Write(BitConverter.GetBytes(perturb.refpos[1]), 0, 4);
stream.Write(BitConverter.GetBytes(perturb.refpos[2]), 0, 4);
stream.Write(BitConverter.GetBytes(perturb.refquat[0]), 0, 4);
stream.Write(BitConverter.GetBytes(perturb.refquat[1]), 0, 4);
stream.Write(BitConverter.GetBytes(perturb.refquat[2]), 0, 4);
stream.Write(BitConverter.GetBytes(perturb.refquat[3]), 0, 4);
lastkey = 0;
break;
// GetImage: send 3*width*height bytes
case Command.GetImage:
RenderToTexture();
stream.Write(offtex.GetRawTextureData(), 0, 3 * offwidth * offheight);
break;
// SaveSnapshot: no data exchange
case Command.SaveSnapshot:
RenderToTexture();
byte[] bytes = offtex.EncodeToPNG();
File.WriteAllBytes(Application.streamingAssetsPath + "/../../" + "img_" +
snapshots + ".png", bytes);
snapshots++;
break;
// SaveVideoframe: no data exchange
case Command.SaveVideoframe:
if (videofile == null)
{
videofile = new FileStream(Application.streamingAssetsPath + "/../../" + "video.raw",
FileMode.Create, FileAccess.Write);
}
RenderToTexture();
videofile.Write(offtex.GetRawTextureData(), 0, 3 * offwidth * offheight);
break;
// SetCamera: receive camera index
case Command.SetCamera:
ReadAll(4);
camindex = BitConverter.ToInt32(buffer, 0);
camindex = Math.Max(-1, Math.Min(ncamera - 1, camindex));
break;
// SetQpos: receive qpos vector
case Command.SetQpos:
steps += 1;
if (nqpos > 0)
{
ReadAll(4 * nqpos);
fixed(byte *qpos = buffer)
{
MJP.SetQpos((float *)qpos);
}
MJP.Kinematics();
UpdateModel();
}
break;
// SetMocap: receive mocap_pos and mocap_quat vectors
case Command.SetMocap:
if (nmocap > 0)
{
ReadAll(28 * nmocap);
fixed(byte *pos = buffer, quat = &buffer[12 * nmocap])
{
MJP.SetMocap((float *)pos, (float *)quat);
}
MJP.Kinematics();
UpdateModel();
}
break;
// GetOVRInput: send Oculus Controller Data (32 Bytes)
case Command.GetOVRInput:
stream.Write(BitConverter.GetBytes(grip), 0, 4);
//stream.Write(BitConverter.GetBytes(0.8f), 0, 4);
//stream.Write(BitConverter.GetBytes(0.65f), 0, 4);
//stream.Write(BitConverter.GetBytes(0.4f), 0, 4);
stream.Write(BitConverter.GetBytes(posbuf[0]), 0, 4);
stream.Write(BitConverter.GetBytes(posbuf[1]), 0, 4);
stream.Write(BitConverter.GetBytes(posbuf[2]), 0, 4);
stream.Write(BitConverter.GetBytes(quatbuf[0]), 0, 4);
stream.Write(BitConverter.GetBytes(quatbuf[1]), 0, 4);
stream.Write(BitConverter.GetBytes(quatbuf[2]), 0, 4);
stream.Write(BitConverter.GetBytes(quatbuf[3]), 0, 4);
break;
// SetTarget: receive target pos
case Command.SetTarget:
ReadAll(12);
steps = 0;
float x = System.BitConverter.ToSingle(buffer.Take(4).ToArray(), 0);
float y = System.BitConverter.ToSingle(buffer.Skip(4).Take(4).ToArray(), 0);
float z = System.BitConverter.ToSingle(buffer.Skip(8).Take(4).ToArray(), 0);
target_pos.position = new Vector3(-10 * x, 10 * z, -10 * y);
break;
// SetTargetStatus: change target material based on distance
case Command.SetTargetStatus:
ReadAll(4);
float value = BitConverter.ToInt32(buffer, 0);
if (value < 0)
{
target_obj.GetComponent <MeshRenderer>().material = normalmat;
}
else
{
target_obj.GetComponent <MeshRenderer>().material = glowmat;
}
break;
// GetSaveStatus: send whether to save demo or not
case Command.GetSaveStatus:
prompt_mode = true;
counter.enabled = false;
title.enabled = false;
subtitle.enabled = false;
prompt.SetActive(true);
break;
}
}
}
