// physics update
unsafe void FixedUpdate()
{
// sync if too late
float time;
MJP.GetTime(&time);
if (time + 0.2 < Time.time)
{
MJP.SetTime(Time.time);
videotime = Time.time;
}
// simulate
int reset;
MJP.Simulate(Time.time, (pause ? 1 : 0), &reset);
// sync if internal reset
if (reset > 0)
{
MJP.SetTime(Time.time);
videotime = Time.time;
}
// save video frames at 60Hz
if (record && Time.time - videotime > 1.0f / 60.0f)
{
RenderToTexture();
videofile.Write(offtex.GetRawTextureData(), 0, 3 * offwidth * offheight);
videotime = Time.time;
}
}
// physics update
unsafe void FixedUpdate()
{
// sync if too late
float time;
MJP.GetTime(&time);
if (time + 0.2 < Time.time)
{
MJP.SetTime(Time.time);
videotime = Time.time;
}
// simulate
int reset;
MJP.Simulate(Time.time, (pause ? 1 : 0), &reset);
// sync if internal reset
if (reset > 0)
{
MJP.SetTime(Time.time);
videotime = Time.time;
}
// save video frames at 60Hz
if (record && Time.time - videotime > 1.0f / 60.0f)
{
videofile.Write(off_render.RenderColor(thecamera).GetRawTextureData(), 0, off_render.GetColorBufferSize());
videotime = Time.time;
}
}
// update Unity representation of MuJoCo model
unsafe private void UpdateModel()
{
MJP.TTransform transform;
// update object states
for (int i = 0; i < nobject; i++)
{
if (objects[i])
{
// set transform and visibility
int visible;
int selected;
MJP.GetObjectState(i, &transform, &visible, &selected);
SetTransform(objects[i], transform);
objects[i].SetActive(visible > 0);
// set emission color
if (selected > 0)
{
objects[i].GetComponent <Renderer>().material.SetColor("_EmissionColor", selcolor);
}
else
{
objects[i].GetComponent <Renderer>().material.SetColor("_EmissionColor", Color.black);
}
}
}
// update camera
MJP.GetCameraState(camindex, &transform);
SetCamera(thecamera, transform);
thecamera.fieldOfView = camfov[camindex + 1];
}
// add camera
private unsafe void AddCamera()
{
if (enable_rendering == false)
{
Destroy(GameObject.Find("PreviewCamera"));
}
else
{
Destroy(GameObject.Find("DummyCamera"));
}
// add camera under root
GameObject camobj = new GameObject("camera");
camobj.layer = LayerMask.NameToLayer("PostProcessing");
camobj.transform.parent = root.transform;
Camera thecamera = camobj.AddComponent <Camera>();
// For Furniture Assembly Environment: remove SITE from the culling mask
thecamera.cullingMask = 1 + (1 << 1) + (1 << 2) + (1 << 4) + (1 << 8);
thecamera.backgroundColor = new Color(1f, 1f, 1f);
thecamera.clearFlags = CameraClearFlags.SolidColor;
Shader segshader = Shader.Find("Unlit/SegmentationColor");
SegmentationShader shadersub = camobj.AddComponent <SegmentationShader>();
DepthShader shaderdepth = camobj.AddComponent <DepthShader>();
// For Furniture Assembly Environment: no post process
//GameObject pp_obj = GameObject.Find("PostPocessing");
//PostProcessLayer pp_layer = camobj.AddComponent<PostProcessLayer>();
//var resources= Resources.FindObjectsOfTypeAll<PostProcessResources>();
//pp_layer.Init(resources[0]);
//pp_layer.volumeTrigger = camobj.transform;
//pp_layer.volumeLayer = 1 << LayerMask.NameToLayer("PostProcessing");
// set field of view, near, far
MJP.TCamera cam;
MJP.GetCamera(-1, &cam);
thecamera.fieldOfView = cam.fov;
// For Furniture Assembly Environment: set znear and zfar independent to model extent.
thecamera.nearClipPlane = 0.01f;
thecamera.farClipPlane = 10f;
//thecamera.nearClipPlane = cam.znear * this.transform.localScale.x;
//thecamera.farClipPlane = cam.zfar * this.transform.localScale.x;
// thecamera.enabled = false;
//camobj.SetActive(enable_rendering);
// set transform
MJP.TTransform transform;
MJP.GetCameraState(-1, &transform);
SetCamera(thecamera, transform);
}
// import materials
private unsafe void ImportMaterials(int nmaterial)
{
// allocate array, find all existing
materials = new Material[nmaterial];
Object[] allmaterials = Resources.FindObjectsOfTypeAll(typeof(Material));
// process materials
for (int i = 0; i < nmaterial; i++)
{
// get material name
StringBuilder name = new StringBuilder(100);
MJP.GetElementName(MJP.TElement.MATERIAL, i, name, 100);
string matname = fileName + "_" + name.ToString();
// find existing material
foreach (Object matt in allmaterials)
{
if (matt != null && matt.name == matname)
{
materials[i] = (Material)matt;
break;
}
}
// not found: create new material
materials[i] = new Material(Shader.Find("Standard"));
// get material descriptor and save
MJP.TMaterial mat;
MJP.GetMaterial(i, &mat);
// set properties
materials[i].name = matname;
materials[i].EnableKeyword("_EMISSION");
materials[i].SetColor("_Color", new Color(mat.color[0], mat.color[1], mat.color[2], mat.color[3]));
materials[i].SetColor("_EmissionColor", new Color(mat.emission, mat.emission, mat.emission, 1));
if (mat.color[3] < 1)
{
materials[i].SetFloat("_Mode", 3.0f);
}
// set texture if present
if (mat.texture >= 0 && importTexture)
{
materials[i].mainTexture = textures[mat.texture];
materials[i].mainTextureScale = new Vector2(mat.texrepeat[0], mat.texrepeat[1]);
}
// create asset in database if not aleady there
if (!AssetDatabase.Contains(materials[i]))
{
AssetDatabase.CreateAsset(materials[i], "Assets/Materials/" + matname + ".mat");
}
}
AssetDatabase.Refresh();
}
// cleanup
void OnApplicationQuit()
{
// free plugin
MJP.Close();
// close file
if (videofile != null)
{
videofile.Close();
}
// free render texture
}
public unsafe void setMocap(NetworkStream stream)
{
if (nmocap > 0)
{
ReadAll(stream, 28 * nmocap);
fixed(byte *pos = buffer, quat = &buffer[12 * nmocap])
{
MJP.SetMocap((float *)pos, (float *)quat);
}
MJP.Kinematics();
UpdateModel();
}
}
public unsafe void setQpos(NetworkStream stream)
{
if (nqpos > 0)
{
ReadAll(stream, 4 * nqpos);
fixed(byte *qpos = buffer)
{
MJP.SetQpos((float *)qpos);
}
MJP.Kinematics();
UpdateModel();
}
}
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;
}
// cleanup
void OnApplicationQuit()
{
// free plugin
MJP.Close();
// close tcp listener
listener.Stop();
// close file
if (videofile != null)
{
videofile.Close();
}
// free render texture
offrt.Release();
}
// add camera
private unsafe void AddCamera()
{
if (enable_rendering == false)
{
Destroy(GameObject.Find("PreviewCamera"));
}
else
{
Destroy(GameObject.Find("DummyCamera"));
}
// add camera under root
GameObject camobj = new GameObject("camera");
camobj.layer = LayerMask.NameToLayer("PostProcessing");
camobj.transform.parent = root.transform;
Camera thecamera = camobj.AddComponent <Camera>();
Shader segshader = Shader.Find("Unlit/SegmentationColor");
SegmentationShader shadersub = camobj.AddComponent <SegmentationShader>();
GameObject pp_obj = GameObject.Find("PostPocessing");
PostProcessLayer pp_layer = camobj.AddComponent <PostProcessLayer>();
var resources = Resources.FindObjectsOfTypeAll <PostProcessResources>();
pp_layer.Init(resources[0]);
pp_layer.volumeTrigger = camobj.transform;
pp_layer.volumeLayer = 1 << LayerMask.NameToLayer("PostProcessing");
// set field of view, near, far
MJP.TCamera cam;
MJP.GetCamera(-1, &cam);
thecamera.fieldOfView = cam.fov;
thecamera.nearClipPlane = cam.znear * this.transform.localScale.x;
thecamera.farClipPlane = cam.zfar * this.transform.localScale.x;
thecamera.enabled = false;
//camobj.SetActive(enable_rendering);
// set transform
MJP.TTransform transform;
MJP.GetCameraState(-1, &transform);
SetCamera(thecamera, transform);
}
private unsafe void ImportLights(int nlight)
{
// allocate array
objects = new GameObject[nlight];
// process objects
for (int i = 0; i < nlight; i++)
{
// get object name
StringBuilder name = new StringBuilder(100);
MJP.GetElementName(MJP.TElement.LIGHT, i, name, 100);
if (name.Length == 0)
{
name.Append("Light "); name.Append(i);
}
// create new GameObject, place under root
objects[i] = new GameObject(name.ToString());
Light lightComp = objects[i].AddComponent <Light>();
objects[i].transform.parent = root.transform;
// get MuJoCo object descriptor
MJP.TLight obj;
MJP.GetLight(i, &obj);
//GetLightState
// set mesh
lightComp.type = obj.directional != 0 ? LightType.Directional : LightType.Point;
lightComp.shadows = obj.castshadow == 0 ? LightShadows.None : LightShadows.Soft;
lightComp.color = new Color(obj.diffuse[0], obj.diffuse[1], obj.diffuse[2]);
lightComp.range = (float)Math.Sqrt(255 / obj.attenuation[2]);
float[] pos = { 0, 0, 0 };
float[] dir = { 0, 0, 0 };
fixed(float *ppos = pos)
fixed(float *pdir = dir)
MJP.GetLightState(i, 0, ppos, pdir);
objects[i].transform.localPosition = new Vector3(-pos[0], pos[2], -pos[1]);
objects[i].transform.rotation = Quaternion.LookRotation(new Vector3(-dir[0], dir[2], -dir[1]), Vector3.up);
}
}
// update Unity representation of MuJoCo model
unsafe private void UpdateModel()
{
MJP.TTransform transform;
// update object states
for (int i = 0; i < nobject; i++)
{
if (objects[i])
{
// set transform and visibility
int visible;
int selected;
MJP.GetObjectState(i, &transform, &visible, &selected);
// For Furniture Assembly Environment: apply new geom position
if (modifiedObjects.ContainsKey(objects[i].name))
{
objects[i].transform.position = modifiedObjects[objects[i].name];
}
else
{
SetTransform(objects[i], transform);
}
objects[i].SetActive(visible > 0);
// set emission color
if (selected > 0)
{
objects[i].GetComponent <Renderer>().material.SetColor("_EmissionColor", selcolor);
}
else
{
objects[i].GetComponent <Renderer>().material.SetColor("_EmissionColor", Color.black);
}
}
}
// update camera
MJP.GetCameraState(camindex, &transform);
SetCamera(thecamera, transform);
thecamera.fieldOfView = camfov[camindex + 1];
}
// add camera
private unsafe void AddCamera()
{
// add camera under root
GameObject camobj = new GameObject("camera");
camobj.transform.parent = root.transform;
Camera thecamera = camobj.AddComponent <Camera>();
// set field of view, near, far
MJP.TCamera cam;
MJP.GetCamera(-1, &cam);
thecamera.fieldOfView = cam.fov;
thecamera.nearClipPlane = cam.znear;
thecamera.farClipPlane = cam.zfar;
// set transform
MJP.TTransform transform;
MJP.GetCameraState(-1, &transform);
SetCamera(thecamera, transform);
}
// import materials
private unsafe void ImportMaterials(int nmaterial)
{
if (default_material == null)
{
Debug.Log("Skipped assigning a material because material is missing");
}
else
{
materials.Add(-1, new Material(default_material));
}
// process materials
for (int i = 0; i < nmaterial; i++)
{
// get material name
StringBuilder name = new StringBuilder(100);
MJP.GetElementName(MJP.TElement.MATERIAL, i, name, 100);
string matname = name.ToString();
Material matt = Resources.Load <Material>("Materials/" + matname);
if (matt == null)
{
Debug.Log("No material for " + matname + " in Resources");
if (default_material == null)
{
Debug.Log("Skipped assigning a material because material is missing");
}
else
{
matt = new Material(default_material);
}
}
if (matt != null)
{
materials.Add(i, matt);
}
}
}
// per-frame update
unsafe void Update()
{
// process input
ProcessKeyboard();
if (Input.mousePresent)
{
ProcessMouse();
}
// update object states
MJP.TTransform transform;
for (int i = 0; i < nobject; i++)
{
if (objects[i])
{
int visible;
int selected;
MJP.GetObjectState(i, &transform, &visible, &selected);
SetTransform(objects[i], transform);
objects[i].SetActive(visible > 0);
// set emission color
if (selected > 0)
{
objects[i].GetComponent <Renderer>().material.SetColor("_EmissionColor", selcolor);
}
else
{
objects[i].GetComponent <Renderer>().material.SetColor("_EmissionColor", Color.black);
}
}
}
// update camera
MJP.GetCameraState(camindex, &transform);
SetCamera(thecamera, transform);
thecamera.fieldOfView = camfov[camindex + 1];
}
// import textures
private unsafe void ImportTextures(int ntexture)
{
// allocate array, find existing
textures = new Texture2D[ntexture];
Object[] alltextures = Resources.FindObjectsOfTypeAll(typeof(Texture2D));
// process textures
for (int i = 0; i < ntexture; i++)
{
// get texture name
StringBuilder name = new StringBuilder(100);
MJP.GetElementName(MJP.TElement.TEXTURE, i, name, 100);
string texname = fileName + "_" + name.ToString();
// get texture descriptor and save
MJP.TTexture tex;
MJP.GetTexture(i, &tex);
// MuJoCo cube texture: use only top piece
if (tex.cube > 0)
{
tex.height = tex.width;
}
// find existing texture
foreach (Object texx in alltextures)
{
if (texx.name == texname)
{
textures[i] = (Texture2D)texx;
// resize if different
if (textures[i].width != tex.width || textures[i].height != tex.height)
{
textures[i].Resize(tex.width, tex.height);
}
break;
}
}
// not found: create new texture
if (textures[i] == null)
{
textures[i] = new Texture2D(tex.width, tex.height);
}
// copy array
Color32[] color = new Color32[tex.width * tex.height];
for (int k = 0; k < tex.width * tex.height; k++)
{
color[k].r = tex.rgb[3 * k];
color[k].g = tex.rgb[3 * k + 1];
color[k].b = tex.rgb[3 * k + 2];
color[k].a = 255;
}
// load data and apply
textures[i].SetPixels32(color);
textures[i].Apply();
// create asset in database if not aleady there
if (!AssetDatabase.Contains(textures[i]))
{
AssetDatabase.CreateAsset(textures[i], "Assets/Textures/" + texname + ".asset");
}
}
AssetDatabase.Refresh();
}
// run importer
private unsafe void RunImport()
{
// adjust global settings
Time.fixedDeltaTime = 0.005f;
PlayerSettings.runInBackground = true;
if (enableRemote)
{
QualitySettings.vSyncCount = (noVSync ? 0 : 1);
}
else
{
QualitySettings.vSyncCount = 1;
}
// disable active cameras
Camera[] activecam = FindObjectsOfType <Camera>();
foreach (Camera ac in activecam)
{
ac.gameObject.SetActive(false);
}
// get filename only (not path or extension)
int i1 = modelFile.LastIndexOf('/');
int i2 = modelFile.LastIndexOf('.');
if (i1 >= 0 && i2 > i1)
{
fileName = modelFile.Substring(i1 + 1, i2 - i1 - 1);
}
else
{
throw new System.Exception("Unexpected model file format");
}
// initialize plugin and load model
MJP.Initialize();
MJP.LoadModel(modelFile);
// get model sizes
MJP.TSize size;
MJP.GetSize(&size);
// save binary model
MakeDirectory("Assets", "StreamingAssets");
MJP.SaveMJB("Assets/StreamingAssets/" + fileName + ".mjb");
// import textures
if (size.ntexture > 0 && importTexture)
{
MakeDirectory("Assets", "Textures");
ImportTextures(size.ntexture);
}
// import materials
if (size.nmaterial > 0)
{
MakeDirectory("Assets", "Materials");
ImportMaterials(size.nmaterial);
}
// create root, destroy old if present
root = GameObject.Find("MuJoCo");
if (root != null)
{
DestroyImmediate(root);
}
root = new GameObject("MuJoCo");
if (root == null)
{
throw new System.Exception("Could not create root MuJoCo object");
}
// add camera to root
AddCamera();
// import renderable objects under root
ImportObjects(size.nobject);
// attach script to root
if (enableRemote)
{
// add remote
MJRemote rem = root.GetComponent <MJRemote>();
if (rem == null)
{
rem = root.AddComponent <MJRemote>();
}
rem.modelFile = fileName + ".mjb";
rem.tcpAddress = tcpAddress;
rem.tcpPort = tcpPort;
// destroy simulate if present
if (root.GetComponent <MJSimulate>())
{
DestroyImmediate(root.GetComponent <MJSimulate>());
}
}
else
{
// add simulate
MJSimulate sim = root.GetComponent <MJSimulate>();
if (sim == null)
{
sim = root.AddComponent <MJSimulate>();
}
sim.modelFile = fileName + ".mjb";
// destroy remote if present
if (root.GetComponent <MJRemote>())
{
DestroyImmediate(root.GetComponent <MJRemote>());
}
}
// close plugin
MJP.Close();
}
// per-frame keyboard input; called from Update
unsafe void ProcessKeyboard()
{
// F1: toggle help
if (Input.GetKeyDown(KeyCode.F1))
{
showhelp++;
if (showhelp > 2)
{
showhelp = 0;
}
}
// G: toggle gravity
else if (Input.GetKeyDown("g"))
{
MJP.TOption opt;
MJP.GetOption(&opt);
opt.gravity = 1 - opt.gravity;
MJP.SetOption(&opt);
}
// E: toggle equality
else if (Input.GetKeyDown("e"))
{
MJP.TOption opt;
MJP.GetOption(&opt);
opt.equality = 1 - opt.equality;
MJP.SetOption(&opt);
}
// L: toggle limit
else if (Input.GetKeyDown("l"))
{
MJP.TOption opt;
MJP.GetOption(&opt);
opt.limit = 1 - opt.limit;
MJP.SetOption(&opt);
}
// R: toggle video recording
else if (Input.GetKeyDown("r"))
{
// recording: close
if (record)
{
record = false;
videofile.Close();
videofile = null;
}
// not recording: open
else
{
record = true;
videofile = new FileStream(Application.streamingAssetsPath + "/../../" + "video_" + videos + ".raw",
FileMode.Create, FileAccess.Write);
videos++;
}
}
// S: save snapshot
else if (Input.GetKeyDown("s"))
{
RenderToTexture();
byte[] bytes = offtex.EncodeToPNG();
File.WriteAllBytes(Application.streamingAssetsPath + "/../../" + "img_" + snapshots + ".png", bytes);
snapshots++;
}
// backspace: reset and sync
else if (Input.GetKeyDown(KeyCode.Backspace))
{
MJP.Reset();
MJP.SetTime(Time.time);
videotime = Time.time;
}
// space: toggle pause and sync
else if (Input.GetKeyDown(KeyCode.Space))
{
pause = !pause;
MJP.SetTime(Time.time);
videotime = Time.time;
}
// Esc: main camera
else if (Input.GetKeyDown(KeyCode.Escape))
{
camindex = -1;
}
// load scene or set model camera
else
{
for (int i = 0; i <= 9; i++)
{
if (Input.GetKeyDown(i.ToString()))
{
// load scene: runtime only
if (Input.GetKey(KeyCode.LeftShift) ||
Input.GetKey(KeyCode.RightShift))
{
#if !UNITY_EDITOR
if (i < SceneManager.sceneCountInBuildSettings)
{
SceneManager.LoadScene(i, LoadSceneMode.Single);
}
#endif
}
// set camera
else
{
if (i < ncamera)
{
camindex = i;
}
}
}
}
}
}
// 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);
}
}
void OnApplicationQuit()
{
MJP.Close();
}
// initialize
unsafe void Start()
{
// For Furniture Assembly Environment: record geom positions
modifiedObjects = new Dictionary <string, Vector3>();
backgrounds = new List <GameObject>();
foreach (GameObject child in SceneManager.GetActiveScene().GetRootGameObjects())
{
if (child.name.StartsWith("Background_"))
{
backgrounds.Add(child);
child.SetActive(false);
}
}
//material_random_parameters["Plastic (Instance)"] = new RandomColor(0.0f, 1.0f, 0.0f, 1.0f, 0.2f, 1.0f);
// set selection color
selcolor = new Color(0.5f, 0.5f, 0.5f, 1);
// preallocate buffer with maximum possible message size
buffersize = 2048; // Math.Max(4, Math.Max(4*nqpos, 28*nmocap));
buffer = new byte[buffersize];
// initialize plugin
// MJP.Initialize();
// MJP.LoadModel(Application.streamingAssetsPath + "/" + modelFile);
// get number of renderable objects, allocate map
MJP.TSize size;
MJP.GetSize(&size);
nqpos = size.nqpos;
nmocap = size.nmocap;
ncamera = size.ncamera;
nobject = size.nobject;
objects = new GameObject[nobject];
// get root
//root = GameObject.Find("MuJoCo");
if (root == null)
{
throw new System.Exception("MuJoCo root object not found");
}
root.transform.localPosition = transform.localPosition;
root.transform.localRotation = transform.localRotation;
root.transform.localScale = transform.localScale;
// get camera under root
int nchild = root.transform.childCount;
for (int i = 0; i < nchild; i++)
{
thecamera = root.transform.GetChild(i).gameObject.GetComponent <Camera>();
if (thecamera != null)
{
// thecamera.enabled = false;
break;
}
}
if (thecamera == null)
{
throw new System.Exception("No camera found under MuJoCo root object");
}
// make map of renderable objects
for (int i = 0; i < nobject; i++)
{
// get object name
StringBuilder name = new StringBuilder(100);
MJP.GetObjectName(i, name, 100);
// find corresponding GameObject
for (int j = 0; j < nchild; j++)
{
if (root.transform.GetChild(j).name == name.ToString())
{
objects[i] = root.transform.GetChild(j).gameObject;
break;
}
}
// set initial state
if (objects[i])
{
MJP.TTransform transform;
int visible;
int selected;
MJP.GetObjectState(i, &transform, &visible, &selected);
SetTransform(objects[i], transform);
objects[i].SetActive(visible > 0);
}
}
// get camera fov and offscreen resolution
camfov = new float[ncamera + 1];
int offwidth = 1280;
int offheight = 720;
for (int i = -1; i < ncamera; i++)
{
MJP.TCamera cam;
MJP.GetCamera(i, &cam);
camfov[i + 1] = cam.fov;
// plugin returns offscreen width and height for all cameras
offwidth = cam.width;
offheight = cam.height;
}
//TODO: The dummy camera and camera added by mjonline import should be merged together
GameObject camobj = GameObject.Find("DummyCamera");
if (camobj != null)
{
dummycamera = camobj.GetComponent <Camera>();
dummycamera.enabled = true;
}
// prepare offscreen rendering
off_render = new OffscreenRenderer(offwidth, offheight);
// synchronize time
MJP.SetTime(Time.time);
//randomizeAppearance();
Debug.Log("New simulation init'd " + offwidth + "x" + offheight);
}
// run importer
private unsafe void RunImport()
{
Resources.UnloadUnusedAssets();
BuildAssetDatabase(modelFile);
// adjust global settings
Time.fixedDeltaTime = 0.005f;
//PlayerSettings.runInBackground = true;
if (enableRemote)
{
QualitySettings.vSyncCount = (noVSync ? 0 : 1);
}
else
{
QualitySettings.vSyncCount = 1;
}
// disable active cameras
/* Camera[] activecam = FindObjectsOfType<Camera>();
* foreach( Camera ac in activecam )
* ac.gameObject.SetActive(false);
*/
fileName = Path.GetFileName(modelFile);
// initialize plugin and load model
MJP.Close();
MJP.Initialize();
MJP.LoadModel(modelFile);
// get model sizes
MJP.TSize size;
MJP.GetSize(&size);
// import materials
//if( size.nmaterial>0 )
{
// MakeDirectory("Assets", "Materials");
ImportMaterials(size.nmaterial);
}
// create root, destroy old if present
root = GameObject.Find("MuJoCo");
if (root != null)
{
Destroy(root);
}
root = new GameObject("MuJoCo");
if (root == null)
{
throw new System.Exception("Could not create root MuJoCo object");
}
root.transform.localPosition = transform.localPosition;
root.transform.localRotation = transform.localRotation;
root.transform.localScale = transform.localScale;
// add camera to root
AddCamera();
// import renderable objects under root
ImportObjects(size.nobject);
// ImportLights(size.nlight);
// attach script to root
if (enableRemote)
{
// add remote
MJRemote extsim = root.GetComponent <MJRemote>();
if (extsim == null)
{
extsim = root.AddComponent <MJRemote>();
}
extsim.root = root;
extsim.modelFile = fileName + ".mjb";
MJTCPInterface tcpif = this.gameObject.GetComponent <MJTCPInterface>();
if (tcpif == null)
{
tcpif = this.gameObject.AddComponent <MJTCPInterface>();
}
tcpif.root = root;
tcpif.tcpAddress = tcpAddress;
tcpif.tcpPort = tcpPort;
// destroy simulate if present
if (root.GetComponent <MJInternalSimulation>())
{
Destroy(root.GetComponent <MJInternalSimulation>());
}
}
else
{
// add simulate
MJInternalSimulation sim = root.GetComponent <MJInternalSimulation>();
if (sim == null)
{
sim = root.AddComponent <MJInternalSimulation>();
}
sim.root = root;
sim.modelFile = fileName + ".mjb";
// destroy remote if present
if (root.GetComponent <MJRemote>())
{
Destroy(root.GetComponent <MJRemote>());
}
// destroy remote if present
if (this.GetComponent <MJTCPInterface>())
{
Destroy(root.GetComponent <MJTCPInterface>());
}
}
// close plugin
// MJP.Close();
}
// initialize
unsafe void Start()
{
// set selection color
selcolor = new Color(0.5f, 0.5f, 0.5f, 1);
// initialize plugin
MJP.Initialize();
MJP.LoadModel(Application.streamingAssetsPath + "/" + modelFile);
// get number of renderable objects, allocate map
MJP.TSize size;
MJP.GetSize(&size);
nqpos = size.nqpos;
nmocap = size.nmocap;
ncamera = size.ncamera;
nobject = size.nobject;
objects = new GameObject[nobject];
// get root
root = GameObject.Find("MuJoCo");
if (root == null)
{
throw new System.Exception("MuJoCo root object not found");
}
// get camera under root
int nchild = root.transform.childCount;
for (int i = 0; i < nchild; i++)
{
thecamera = root.transform.GetChild(i).gameObject.GetComponent <Camera>();
if (thecamera != null)
{
break;
}
}
if (thecamera == null)
{
throw new System.Exception("No camera found under MuJoCo root object");
}
// make map of renderable objects
for (int i = 0; i < nobject; i++)
{
// get object name
StringBuilder name = new StringBuilder(100);
MJP.GetObjectName(i, name, 100);
// find corresponding GameObject
for (int j = 0; j < nchild; j++)
{
if (root.transform.GetChild(j).name == name.ToString())
{
objects[i] = root.transform.GetChild(j).gameObject;
break;
}
}
// set initial state
if (objects[i])
{
MJP.TTransform transform;
int visible;
int selected;
MJP.GetObjectState(i, &transform, &visible, &selected);
SetTransform(objects[i], transform);
objects[i].SetActive(visible > 0);
}
}
// get camera fov and offscreen resolution
camfov = new float[ncamera + 1];
for (int i = -1; i < ncamera; i++)
{
MJP.TCamera cam;
MJP.GetCamera(i, &cam);
camfov[i + 1] = cam.fov;
// plugin returns offscreen width and height for all cameras
offwidth = cam.width;
offheight = cam.height;
}
// prepare offscreen rendering
offtex = new Texture2D(offwidth, offheight, TextureFormat.RGB24, false);
offrt = new RenderTexture(offwidth, offheight, 24);
offrt.width = offwidth;
offrt.height = offheight;
offrt.Create();
// synchronize time
MJP.SetTime(Time.time);
// preallocate buffer with maximum possible message size
buffersize = Math.Max(4, Math.Max(4 * nqpos, 28 * nmocap));
buffer = new byte[buffersize];
// start listening for connections
listener = new TcpListener(System.Net.IPAddress.Parse(tcpAddress), tcpPort);
listener.Start();
}
// import renderable objects
private unsafe void ImportObjects(int nobject)
{
string cwd = Path.GetDirectoryName(modelFile);
// make primitives
PrimitiveType[] ptypes =
{
PrimitiveType.Plane,
PrimitiveType.Sphere,
PrimitiveType.Cylinder,
PrimitiveType.Cube
};
GameObject[] primitives = new GameObject[4];
for (int i = 0; i < 4; i++)
{
primitives[i] = GameObject.CreatePrimitive(ptypes[i]);
}
// allocate array
objects = new GameObject[nobject];
// process objects
for (int i = 0; i < nobject; i++)
{
// get object name
StringBuilder name = new StringBuilder(100);
MJP.GetObjectName(i, name, 100);
// create new GameObject, place under root
objects[i] = new GameObject(name.ToString());
MeshFilter filt = objects[i].AddComponent <MeshFilter>();
MeshRenderer rend = objects[i].AddComponent <MeshRenderer>();
InstancedColor colors = objects[i].AddComponent <InstancedColor>();
objects[i].transform.parent = root.transform;
// get MuJoCo object descriptor
MJP.TObject obj;
MJP.GetObject(i, &obj);
// For Furniture Assembly Environment: do not visualize site
if (obj.category == (int)MJP.TCategory.SITE && !objects[i].name.Contains("conn"))
{
objects[i].layer = 9;
}
if (objects[i].name.StartsWith("noviz", StringComparison.Ordinal))
{
objects[i].layer = 10;
}
if (objects[i].name.StartsWith("floor", StringComparison.Ordinal))
{
objects[i].layer = 10;
}
// set mesh
switch ((MJP.TGeom)obj.geomtype)
{
case MJP.TGeom.PLANE:
filt.sharedMesh = primitives[0].GetComponent <MeshFilter>().sharedMesh;
break;
case MJP.TGeom.SPHERE:
filt.sharedMesh = primitives[1].GetComponent <MeshFilter>().sharedMesh;
break;
case MJP.TGeom.CYLINDER:
filt.sharedMesh = primitives[2].GetComponent <MeshFilter>().sharedMesh;
break;
case MJP.TGeom.BOX:
filt.sharedMesh = primitives[3].GetComponent <MeshFilter>().sharedMesh;
break;
case MJP.TGeom.HFIELD:
int nrow = obj.hfield_nrow;
int ncol = obj.hfield_ncol;
int r, c;
// allocate
Vector3[] hfvertices = new Vector3[nrow * ncol + 4 * nrow + 4 * ncol];
Vector2[] hfuv = new Vector2[nrow * ncol + 4 * nrow + 4 * ncol];
int[] hffaces0 = new int[3 * 2 * (nrow - 1) * (ncol - 1)];
int[] hffaces1 = new int[3 * (4 * (nrow - 1) + 4 * (ncol - 1))];
// vertices and uv: surface
for (r = 0; r < nrow; r++)
{
for (c = 0; c < ncol; c++)
{
int k = r * ncol + c;
float wc = c / (float)(ncol - 1);
float wr = r / (float)(nrow - 1);
hfvertices[k].Set(-(wc - 0.5f), obj.hfield_data[k], -(wr - 0.5f));
hfuv[k].Set(wc, wr);
}
}
// vertices and uv: front and back
for (r = 0; r < nrow; r += (nrow - 1))
{
for (c = 0; c < ncol; c++)
{
int k = nrow * ncol + 2 * ((r > 0?ncol:0) + c);
float wc = c / (float)(ncol - 1);
float wr = r / (float)(nrow - 1);
hfvertices[k].Set(-(wc - 0.5f), -0.5f, -(wr - 0.5f));
hfuv[k].Set(wc, 0);
hfvertices[k + 1].Set(-(wc - 0.5f), obj.hfield_data[r * ncol + c], -(wr - 0.5f));
hfuv[k + 1].Set(wc, 1);
}
}
// vertices and uv: left and right
for (c = 0; c < ncol; c += (ncol - 1))
{
for (r = 0; r < nrow; r++)
{
int k = nrow * ncol + 4 * ncol + 2 * ((c > 0?nrow:0) + r);
float wc = c / (float)(ncol - 1);
float wr = r / (float)(nrow - 1);
hfvertices[k].Set(-(wc - 0.5f), -0.5f, -(wr - 0.5f));
hfuv[k].Set(wr, 0);
hfvertices[k + 1].Set(-(wc - 0.5f), obj.hfield_data[r * ncol + c], -(wr - 0.5f));
hfuv[k + 1].Set(wr, 1);
}
}
// faces: surface
for (r = 0; r < nrow - 1; r++)
{
for (c = 0; c < ncol - 1; c++)
{
int f = r * (ncol - 1) + c;
int k = r * ncol + c;
// first face in rectangle
hffaces0[3 * 2 * f] = k;
hffaces0[3 * 2 * f + 2] = k + 1;
hffaces0[3 * 2 * f + 1] = k + ncol + 1;
// second face in rectangle
hffaces0[3 * 2 * f + 3] = k;
hffaces0[3 * 2 * f + 5] = k + ncol + 1;
hffaces0[3 * 2 * f + 4] = k + ncol;
}
}
// faces: front and back
for (r = 0; r < 2; r++)
{
for (c = 0; c < ncol - 1; c++)
{
int f = ((r > 0?(ncol - 1):0) + c);
int k = nrow * ncol + 2 * ((r > 0?ncol:0) + c);
// first face in rectangle
hffaces1[3 * 2 * f] = k;
hffaces1[3 * 2 * f + 2] = k + (r > 0 ? 1 : 3);
hffaces1[3 * 2 * f + 1] = k + (r > 0 ? 3 : 1);
// second face in rectangle
hffaces1[3 * 2 * f + 3] = k;
hffaces1[3 * 2 * f + 5] = k + (r > 0 ? 3 : 2);
hffaces1[3 * 2 * f + 4] = k + (r > 0 ? 2 : 3);
}
}
// faces: left and right
for (c = 0; c < 2; c++)
{
for (r = 0; r < nrow - 1; r++)
{
int f = 2 * (ncol - 1) + ((c > 0?(nrow - 1):0) + r);
int k = nrow * ncol + 4 * ncol + 2 * ((c > 0?nrow:0) + r);
// first face in rectangle
hffaces1[3 * 2 * f] = k;
hffaces1[3 * 2 * f + 2] = k + (c > 0 ? 3 : 1);
hffaces1[3 * 2 * f + 1] = k + (c > 0 ? 1 : 3);
// second face in rectangle
hffaces1[3 * 2 * f + 3] = k;
hffaces1[3 * 2 * f + 5] = k + (c > 0 ? 2 : 3);
hffaces1[3 * 2 * f + 4] = k + (c > 0 ? 3 : 2);
}
}
Debug.Log(ncol);
Debug.Log(nrow);
Debug.Log(Mathf.Min(hffaces1));
Debug.Log(Mathf.Max(hffaces1));
// create mesh with automatic normals and tangents
filt.sharedMesh = new Mesh();
filt.sharedMesh.vertices = hfvertices;
filt.sharedMesh.uv = hfuv;
filt.sharedMesh.subMeshCount = 2;
filt.sharedMesh.SetTriangles(hffaces0, 0);
filt.sharedMesh.SetTriangles(hffaces1, 1);
filt.sharedMesh.RecalculateNormals();
filt.sharedMesh.RecalculateTangents();
// set name
StringBuilder hname = new StringBuilder(100);
MJP.GetElementName(MJP.TElement.HFIELD, obj.dataid, hname, 100);
filt.sharedMesh.name = hname.ToString();
break;
case MJP.TGeom.CAPSULE:
case MJP.TGeom.MESH:
// reuse shared mesh from earlier object
if (obj.mesh_shared >= 0)
{
filt.sharedMesh = objects[obj.mesh_shared].GetComponent <MeshFilter>().sharedMesh;
}
// create new mesh
else
{
string meshName;
// set name
if ((MJP.TGeom)obj.geomtype == MJP.TGeom.CAPSULE)
{
meshName = "Capsule mesh";
}
else
{
StringBuilder mname = new StringBuilder(100);
MJP.GetElementName(MJP.TElement.MESH, obj.dataid, mname, 100);
meshName = mname.ToString();
}
{
// copy vertices, normals, uv
Vector3[] vertices = new Vector3[obj.mesh_nvertex];
Vector3[] normals = new Vector3[obj.mesh_nvertex];
Vector2[] uv = new Vector2[obj.mesh_nvertex];
for (int k = 0; k < obj.mesh_nvertex; k++)
{
vertices[k].Set(-obj.mesh_position[3 * k],
obj.mesh_position[3 * k + 2],
-obj.mesh_position[3 * k + 1]);
normals[k].Set(-obj.mesh_normal[3 * k],
obj.mesh_normal[3 * k + 2],
-obj.mesh_normal[3 * k + 1]);
uv[k].Set(obj.mesh_texcoord[2 * k],
obj.mesh_texcoord[2 * k + 1]);
}
// copy faces
int[] faces = new int[3 * obj.mesh_nface];
for (int k = 0; k < obj.mesh_nface; k++)
{
faces[3 * k] = obj.mesh_face[3 * k];
faces[3 * k + 1] = obj.mesh_face[3 * k + 2];
faces[3 * k + 2] = obj.mesh_face[3 * k + 1];
}
// number of verices can be modified by uncompressed mesh
int nvert = obj.mesh_nvertex;
// replace with uncompressed mesh when UV needs to be recomputed
// ( recomputeUV && (MJP.TGeom)obj.geomtype==MJP.TGeom.MESH )
{
// make temporary mesh
Mesh temp = new Mesh();
temp.vertices = vertices;
temp.normals = normals;
temp.triangles = faces;
// generate uncompressed UV unwrapping
/* Vector2[] UV = Unwrapping.GeneratePerTriangleUV(temp);
* int N = UV.GetLength(0)/3;
* if( N!=obj.mesh_nface )
* throw new System.Exception("Unexpected number of faces");
* nvert = 3*N;*/
int N = obj.mesh_nface;
nvert = 3 * N;
// create corresponding uncompressed vertices, normals, faces
Vector3[] Vertex = new Vector3[3 * N];
Vector3[] Normal = new Vector3[3 * N];
int[] Face = new int[3 * N];
for (int k = 0; k < N; k++)
{
Vertex[3 * k] = vertices[faces[3 * k]];
Vertex[3 * k + 1] = vertices[faces[3 * k + 1]];
Vertex[3 * k + 2] = vertices[faces[3 * k + 2]];
Normal[3 * k] = normals[faces[3 * k]];
Normal[3 * k + 1] = normals[faces[3 * k + 1]];
Normal[3 * k + 2] = normals[faces[3 * k + 2]];
Face[3 * k] = 3 * k;
Face[3 * k + 1] = 3 * k + 1;
Face[3 * k + 2] = 3 * k + 2;
}
// create uncompressed mesh
filt.sharedMesh = new Mesh();
filt.sharedMesh.indexFormat = UnityEngine.Rendering.IndexFormat.UInt32;
filt.sharedMesh.vertices = Vertex;
filt.sharedMesh.normals = Normal;
filt.sharedMesh.triangles = Face;
filt.sharedMesh.name = meshName;
// filt.sharedMesh.uv = UV;
}
// otherwise create mesh directly
/* else
* {
* filt.sharedMesh = new Mesh();
* filt.sharedMesh.indexFormat = UnityEngine.Rendering.IndexFormat.UInt32;
* filt.sharedMesh.vertices = vertices;
* filt.sharedMesh.normals = normals;
* filt.sharedMesh.triangles = faces;
* filt.sharedMesh.uv = uv;
* filt.sharedMesh.name = meshName;
* filt.sharedMesh.RecalculateNormals(30);
* }*/
// optionally recompute normals for meshes
/* if (recomputeNormal && (MJP.TGeom)obj.geomtype == MJP.TGeom.MESH)
* filt.sharedMesh.RecalculateNormals(60);*/
filt.sharedMesh.RecalculateNormals(25);
// always calculate tangents (MuJoCo does not support tangents)
filt.sharedMesh.RecalculateTangents();
}
// print error if number of vertices or faces is over 65535
/* if( obj.mesh_nface>65535 || nvert>65535 )
* Debug.LogError("MESH TOO BIG: " + filt.sharedMesh.name +
* ", vertices " + nvert + ", faces " + obj.mesh_nface);*/
}
break;
}
//TODO: Set segmentation color with Material.SetColor("_SegColor")
// existing material
byte segmentation = 0;
// Try to get segmentation id. If nothing is found, the object will remain background (0)
if (meshSegmentMap.TryGetValue(name.ToString(), out segmentation) == false)
{
meshSegmentMap.TryGetValue(filt.sharedMesh.name, out segmentation);
}
Material base_material = null;
if (materials.TryGetValue(obj.material, out base_material))
{
rend.sharedMaterial = base_material;
colors.Diffuse = new Color(obj.color[0], obj.color[1], obj.color[2], obj.color[3]);
colors.Segmentation = new Color32(segmentation, segmentation, segmentation, segmentation);
}
else // Missing material (shouldn't be possible?)
{
Debug.Log("Couldn't find a Material for id:" + obj.material);
}
// get MuJoCo object transform and set in Unity
MJP.TTransform transform;
int visible;
int selected;
MJP.GetObjectState(i, &transform, &visible, &selected);
SetTransform(objects[i], transform);
}
// delete primitives
for (int i = 0; i < 4; i++)
{
Destroy(primitives[i]);
}
}
// import renderable objects
private unsafe void ImportObjects(int nobject)
{
// make primitives
PrimitiveType[] ptypes =
{
PrimitiveType.Plane,
PrimitiveType.Sphere,
PrimitiveType.Cylinder,
PrimitiveType.Cube
};
GameObject[] primitives = new GameObject[4];
for (int i = 0; i < 4; i++)
{
primitives[i] = GameObject.CreatePrimitive(ptypes[i]);
}
// allocate array
objects = new GameObject[nobject];
// process objects
for (int i = 0; i < nobject; i++)
{
// get object name
StringBuilder name = new StringBuilder(100);
MJP.GetObjectName(i, name, 100);
// create new GameObject, place under root
objects[i] = new GameObject(name.ToString());
objects[i].AddComponent <MeshFilter>();
objects[i].AddComponent <MeshRenderer>();
objects[i].transform.parent = root.transform;
// get components
MeshFilter filt = objects[i].GetComponent <MeshFilter>();
MeshRenderer rend = objects[i].GetComponent <MeshRenderer>();
// get MuJoCo object descriptor
MJP.TObject obj;
MJP.GetObject(i, &obj);
// set mesh
switch ((MJP.TGeom)obj.geomtype)
{
case MJP.TGeom.PLANE:
filt.sharedMesh = primitives[0].GetComponent <MeshFilter>().sharedMesh;
break;
case MJP.TGeom.SPHERE:
filt.sharedMesh = primitives[1].GetComponent <MeshFilter>().sharedMesh;
break;
case MJP.TGeom.CYLINDER:
filt.sharedMesh = primitives[2].GetComponent <MeshFilter>().sharedMesh;
break;
case MJP.TGeom.BOX:
filt.sharedMesh = primitives[3].GetComponent <MeshFilter>().sharedMesh;
break;
case MJP.TGeom.HFIELD:
int nrow = obj.hfield_nrow;
int ncol = obj.hfield_ncol;
int r, c;
// allocate
Vector3[] hfvertices = new Vector3[nrow * ncol + 4 * nrow + 4 * ncol];
Vector2[] hfuv = new Vector2[nrow * ncol + 4 * nrow + 4 * ncol];
int[] hffaces0 = new int[3 * 2 * (nrow - 1) * (ncol - 1)];
int[] hffaces1 = new int[3 * (4 * (nrow - 1) + 4 * (ncol - 1))];
// vertices and uv: surface
for (r = 0; r < nrow; r++)
{
for (c = 0; c < ncol; c++)
{
int k = r * ncol + c;
float wc = c / (float)(ncol - 1);
float wr = r / (float)(nrow - 1);
hfvertices[k].Set(-(wc - 0.5f), obj.hfield_data[k], -(wr - 0.5f));
hfuv[k].Set(wc, wr);
}
}
// vertices and uv: front and back
for (r = 0; r < nrow; r += (nrow - 1))
{
for (c = 0; c < ncol; c++)
{
int k = nrow * ncol + 2 * ((r > 0?ncol:0) + c);
float wc = c / (float)(ncol - 1);
float wr = r / (float)(nrow - 1);
hfvertices[k].Set(-(wc - 0.5f), -0.5f, -(wr - 0.5f));
hfuv[k].Set(wc, 0);
hfvertices[k + 1].Set(-(wc - 0.5f), obj.hfield_data[r * ncol + c], -(wr - 0.5f));
hfuv[k + 1].Set(wc, 1);
}
}
// vertices and uv: left and right
for (c = 0; c < ncol; c += (ncol - 1))
{
for (r = 0; r < nrow; r++)
{
int k = nrow * ncol + 4 * ncol + 2 * ((c > 0?nrow:0) + r);
float wc = c / (float)(ncol - 1);
float wr = r / (float)(nrow - 1);
hfvertices[k].Set(-(wc - 0.5f), -0.5f, -(wr - 0.5f));
hfuv[k].Set(wr, 0);
hfvertices[k + 1].Set(-(wc - 0.5f), obj.hfield_data[r * ncol + c], -(wr - 0.5f));
hfuv[k + 1].Set(wr, 1);
}
}
// faces: surface
for (r = 0; r < nrow - 1; r++)
{
for (c = 0; c < ncol - 1; c++)
{
int f = r * (ncol - 1) + c;
int k = r * ncol + c;
// first face in rectangle
hffaces0[3 * 2 * f] = k;
hffaces0[3 * 2 * f + 2] = k + 1;
hffaces0[3 * 2 * f + 1] = k + ncol + 1;
// second face in rectangle
hffaces0[3 * 2 * f + 3] = k;
hffaces0[3 * 2 * f + 5] = k + ncol + 1;
hffaces0[3 * 2 * f + 4] = k + ncol;
}
}
// faces: front and back
for (r = 0; r < 2; r++)
{
for (c = 0; c < ncol - 1; c++)
{
int f = ((r > 0?(ncol - 1):0) + c);
int k = nrow * ncol + 2 * ((r > 0?ncol:0) + c);
// first face in rectangle
hffaces1[3 * 2 * f] = k;
hffaces1[3 * 2 * f + 2] = k + (r > 0 ? 1 : 3);
hffaces1[3 * 2 * f + 1] = k + (r > 0 ? 3 : 1);
// second face in rectangle
hffaces1[3 * 2 * f + 3] = k;
hffaces1[3 * 2 * f + 5] = k + (r > 0 ? 3 : 2);
hffaces1[3 * 2 * f + 4] = k + (r > 0 ? 2 : 3);
}
}
// faces: left and right
for (c = 0; c < 2; c++)
{
for (r = 0; r < nrow - 1; r++)
{
int f = 2 * (ncol - 1) + ((c > 0?(nrow - 1):0) + r);
int k = nrow * ncol + 4 * ncol + 2 * ((c > 0?nrow:0) + r);
// first face in rectangle
hffaces1[3 * 2 * f] = k;
hffaces1[3 * 2 * f + 2] = k + (c > 0 ? 3 : 1);
hffaces1[3 * 2 * f + 1] = k + (c > 0 ? 1 : 3);
// second face in rectangle
hffaces1[3 * 2 * f + 3] = k;
hffaces1[3 * 2 * f + 5] = k + (c > 0 ? 2 : 3);
hffaces1[3 * 2 * f + 4] = k + (c > 0 ? 3 : 2);
}
}
Debug.Log(ncol);
Debug.Log(nrow);
Debug.Log(Mathf.Min(hffaces1));
Debug.Log(Mathf.Max(hffaces1));
// create mesh with automatic normals and tangents
filt.sharedMesh = new Mesh();
filt.sharedMesh.vertices = hfvertices;
filt.sharedMesh.uv = hfuv;
filt.sharedMesh.subMeshCount = 2;
filt.sharedMesh.SetTriangles(hffaces0, 0);
filt.sharedMesh.SetTriangles(hffaces1, 1);
filt.sharedMesh.RecalculateNormals();
filt.sharedMesh.RecalculateTangents();
// set name
StringBuilder hname = new StringBuilder(100);
MJP.GetElementName(MJP.TElement.HFIELD, obj.dataid, hname, 100);
filt.sharedMesh.name = hname.ToString();
break;
case MJP.TGeom.CAPSULE:
case MJP.TGeom.MESH:
// reuse shared mesh from earlier object
if (obj.mesh_shared >= 0)
{
filt.sharedMesh = objects[obj.mesh_shared].GetComponent <MeshFilter>().sharedMesh;
}
// create new mesh
else
{
// copy vertices, normals, uv
Vector3[] vertices = new Vector3[obj.mesh_nvertex];
Vector3[] normals = new Vector3[obj.mesh_nvertex];
Vector2[] uv = new Vector2[obj.mesh_nvertex];
for (int k = 0; k < obj.mesh_nvertex; k++)
{
vertices[k].Set(-obj.mesh_position[3 * k],
obj.mesh_position[3 * k + 2],
-obj.mesh_position[3 * k + 1]);
normals[k].Set(-obj.mesh_normal[3 * k],
obj.mesh_normal[3 * k + 2],
-obj.mesh_normal[3 * k + 1]);
uv[k].Set(obj.mesh_texcoord[2 * k],
obj.mesh_texcoord[2 * k + 1]);
}
// copy faces
int[] faces = new int[3 * obj.mesh_nface];
for (int k = 0; k < obj.mesh_nface; k++)
{
faces[3 * k] = obj.mesh_face[3 * k];
faces[3 * k + 1] = obj.mesh_face[3 * k + 2];
faces[3 * k + 2] = obj.mesh_face[3 * k + 1];
}
// number of verices can be modified by uncompressed mesh
int nvert = obj.mesh_nvertex;
// replace with uncompressed mesh when UV needs to be recomputed
if (recomputeUV && (MJP.TGeom)obj.geomtype == MJP.TGeom.MESH)
{
// make temporary mesh
Mesh temp = new Mesh();
temp.vertices = vertices;
temp.normals = normals;
temp.triangles = faces;
// generate uncompressed UV unwrapping
Vector2[] UV = Unwrapping.GeneratePerTriangleUV(temp);
int N = UV.GetLength(0) / 3;
if (N != obj.mesh_nface)
{
throw new System.Exception("Unexpected number of faces");
}
nvert = 3 * N;
// create corresponding uncompressed vertices, normals, faces
Vector3[] Vertex = new Vector3[3 * N];
Vector3[] Normal = new Vector3[3 * N];
int[] Face = new int[3 * N];
for (int k = 0; k < N; k++)
{
Vertex[3 * k] = vertices[faces[3 * k]];
Vertex[3 * k + 1] = vertices[faces[3 * k + 1]];
Vertex[3 * k + 2] = vertices[faces[3 * k + 2]];
Normal[3 * k] = normals[faces[3 * k]];
Normal[3 * k + 1] = normals[faces[3 * k + 1]];
Normal[3 * k + 2] = normals[faces[3 * k + 2]];
Face[3 * k] = 3 * k;
Face[3 * k + 1] = 3 * k + 1;
Face[3 * k + 2] = 3 * k + 2;
}
// create uncompressed mesh
filt.sharedMesh = new Mesh();
filt.sharedMesh.vertices = Vertex;
filt.sharedMesh.normals = Normal;
filt.sharedMesh.triangles = Face;
filt.sharedMesh.uv = UV;
}
// otherwise create mesh directly
else
{
filt.sharedMesh = new Mesh();
filt.sharedMesh.vertices = vertices;
filt.sharedMesh.normals = normals;
filt.sharedMesh.triangles = faces;
filt.sharedMesh.uv = uv;
}
// optionally recompute normals for meshes
if (recomputeNormal && (MJP.TGeom)obj.geomtype == MJP.TGeom.MESH)
{
filt.sharedMesh.RecalculateNormals();
}
// always calculate tangents (MuJoCo does not support tangents)
filt.sharedMesh.RecalculateTangents();
// set name
if ((MJP.TGeom)obj.geomtype == MJP.TGeom.CAPSULE)
{
filt.sharedMesh.name = "Capsule mesh";
}
else
{
StringBuilder mname = new StringBuilder(100);
MJP.GetElementName(MJP.TElement.MESH, obj.dataid, mname, 100);
filt.sharedMesh.name = mname.ToString();
}
// print error if number of vertices or faces is over 65535
if (obj.mesh_nface > 65535 || nvert > 65535)
{
Debug.LogError("MESH TOO BIG: " + filt.sharedMesh.name +
", vertices " + nvert + ", faces " + obj.mesh_nface);
}
}
break;
}
// existing material
if (obj.material >= 0)
{
// not modified
if (obj.color[0] == 0.5f && obj.color[1] == 0.5f && obj.color[2] == 0.5f && obj.color[3] == 1)
{
rend.sharedMaterial = materials[obj.material];
}
// color override
else
{
rend.sharedMaterial = new Material(materials[obj.material]);
AdjustMaterial(rend.sharedMaterial, obj.color[0], obj.color[1], obj.color[2], obj.color[3]);
}
}
// new material
else
{
rend.sharedMaterial = new Material(Shader.Find("Standard"));
AdjustMaterial(rend.sharedMaterial, obj.color[0], obj.color[1], obj.color[2], obj.color[3]);
}
// get MuJoCo object transform and set in Unity
MJP.TTransform transform;
int visible;
int selected;
MJP.GetObjectState(i, &transform, &visible, &selected);
SetTransform(objects[i], transform);
}
// delete primitives
for (int i = 0; i < 4; i++)
{
DestroyImmediate(primitives[i]);
}
AssetDatabase.Refresh();
}
// 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;
}
}
}
// initialize
unsafe void Start()
{
// set selection color
selcolor = new Color(0.5f, 0.5f, 0.5f, 1);
// initialize plugin
//// MJP.Initialize();
// MJP.LoadModel(Application.streamingAssetsPath + "/" + modelFile);
// get number of renderable objects, allocate map
MJP.TSize size;
MJP.GetSize(&size);
ncamera = size.ncamera;
nobject = size.nobject;
objects = new GameObject[nobject];
// get root
// root = GameObject.Find("MuJoCo");
if (root == null)
{
throw new System.Exception("MuJoCo root object not found");
}
// get camera under root
int nchild = root.transform.childCount;
for (int i = 0; i < nchild; i++)
{
thecamera = root.transform.GetChild(i).gameObject.GetComponent <Camera>();
if (thecamera != null)
{
break;
}
}
if (thecamera == null)
{
throw new System.Exception("No camera found under MuJoCo root object");
}
// make map of renderable objects
for (int i = 0; i < nobject; i++)
{
// get object name
StringBuilder name = new StringBuilder(100);
MJP.GetObjectName(i, name, 100);
// find corresponding GameObject
for (int j = 0; j < nchild; j++)
{
if (root.transform.GetChild(j).name == name.ToString())
{
objects[i] = root.transform.GetChild(j).gameObject;
break;
}
}
// set initial state
if (objects[i])
{
MJP.TTransform transform;
int visible;
int selected;
MJP.GetObjectState(i, &transform, &visible, &selected);
SetTransform(objects[i], transform);
objects[i].SetActive(visible > 0);
}
}
int offwidth = 1280;
int offheight = 720;
// get camera fov and offscreen resolution
camfov = new float[ncamera + 1];
for (int i = -1; i < ncamera; i++)
{
MJP.TCamera cam;
MJP.GetCamera(i, &cam);
camfov[i + 1] = cam.fov;
// plugin returns offscreen width and height for all cameras
offwidth = cam.width;
offheight = cam.height;
}
off_render = new OffscreenRenderer(offwidth, offheight);
// synchronize time
MJP.SetTime(Time.time);
videotime = Time.time;
}
// 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;
}
}
}
