/// <summary>
/// Raycast the annotation point to intersect with geometry
/// </summary>
protected bool Raycast(SE3 matrix, Vector2 anno, out RaycastHit hitInfo)
{
Vector3 direction = matrix.Rotation * Camera.Unproj(anno);
bool result;
if (OnPlane)
{
Vector3 p;
result = Room.Raycast(matrix.Translation, matrix.Rotation * Camera.Unproj(anno), out p);
hitInfo = new RaycastHit
{
point = p
};
}
else
{
result = Physics.Raycast(matrix.Translation, direction, out hitInfo, 300.0f, SpatialMappingManager.Instance.LayerMask);
}
hitInfo.point += ToolOffset * direction;
hitInfo.point += AnnotationXOffset * Vector3.right;
hitInfo.point += AnnotationYOffset * Vector3.forward;
hitInfo.point += AnnotationZOffset * Vector3.up;
ObjectFactory.NewRay(Rays, matrix.Translation, hitInfo.point, Color.white, 0.005f);
return(result);
}
private static int ParseFrame(byte[] data, int offset, SlamMapEventData obj)
{
int index = offset;
uint id = BitConverter.ToUInt32(data, index);
index += 4;
double time = BitConverter.ToDouble(data, index);
index += 8;
SE3 Tcw = new SE3();
SE3 Tbw = new SE3();
Vector3 v = new Vector3();
Vector3 bg = new Vector3();
Vector3 ba = new Vector3();
index = ParseSE3(data, index, Tcw);
index = ParseSE3(data, index, Tbw);
index = ParseVector3(data, index, v);
index = ParseVector3(data, index, bg);
index = ParseVector3(data, index, ba);
double scale = BitConverter.ToDouble(data, index);
index += 8;
obj._Frame = new Frame(id, time, Tcw, Tbw, v, bg, ba, scale);
return(index - offset);
}
private static int ParseSE3(byte[] data, int offset, SE3 obj)
{
int index = offset;
obj.Translation.X = BitConverter.ToDouble(data, index);
index += 8;
obj.Translation.Y = BitConverter.ToDouble(data, index);
index += 8;
obj.Translation.Z = BitConverter.ToDouble(data, index);
index += 8;
double q1 = BitConverter.ToDouble(data, index);
index += 8;
double q2 = BitConverter.ToDouble(data, index);
index += 8;
double q3 = BitConverter.ToDouble(data, index);
index += 8;
double q4 = BitConverter.ToDouble(data, index);
index += 8;
obj.SO3.Quaternion = new Quaternion(q1, q2, q3, q4);
return(index);
}
private void OnTriggerStay(Collider other)
{
if (LayerMask.LayerToName(gameObject.layer) == "Damage")
{
return;
}
if (other.tag != "Boundary" && (other.tag == "Enemy" || other.tag == "Enemy_Bullet"))
{
Debug.Log(player_current_hp);
player_current_hp -= 1;
if (player_current_hp <= 1.1f && player_current_hp >= 0.9f)
{
SE3.Play();
}
hpSlider.value = player_current_hp; //Valueの値をPyHPの値にする
if (player_current_hp <= 0)
{
Instantiate(explosion, transform.position, transform.rotation);
GameObject[] child = GameObject.FindGameObjectsWithTag("Child");
for (int i = 0; i < child.Length; i++)
{
Instantiate(explosion, child[i].transform.position, child[i].transform.rotation);
Destroy(child[i].gameObject);
}
//switch (Parameter.STAGE)
// {
// case 1:
// GameObject.FindWithTag("GameController").GetComponent<GameController_Stage1>().gameover();
// break;
// default:
GameObject.FindWithTag("GameController").GetComponent <GameController>().gameover();
// break;
// }
Destroy(gameObject);
}
else
{
SE2.Play();
StartCoroutine("Damage");
}
}
}
public List <Frame> GetFrames()
{
List <Frame> frames = new List <Frame>();
IStructureArray dataStruct = _MatFile["data"].Value as IStructureArray;
IArray timePre = (dataStruct["PreOptimization", 0] as IStructureArray)["time", 0];
IArray idPre = (dataStruct["PreOptimization", 0] as IStructureArray)["id", 0];
IArray T_cam_worldPre = (dataStruct["PreOptimization", 0] as IStructureArray)["T_cam_world", 0];
IArray T_base_worldPre = (dataStruct["PreOptimization", 0] as IStructureArray)["T_base_world", 0];
IArray velocityPre = (dataStruct["PreOptimization", 0] as IStructureArray)["velocity", 0];
IArray bgPre = (dataStruct["PreOptimization", 0] as IStructureArray)["bias_gyroscope", 0];
IArray baPre = (dataStruct["PreOptimization", 0] as IStructureArray)["bias_accelerometer", 0];
IArray scalePre = (dataStruct["PreOptimization", 0] as IStructureArray)["scale", 0];
for (int i = 0; i < timePre.Dimensions[1]; i++)
{
double[,] matrixTcw = new double[4, 4];
double[,] matrixTbw = new double[4, 4];
Vector3 v = new Vector3();
Vector3 bg = new Vector3();
Vector3 ba = new Vector3();
for (int j = 0; j < 3; j++)
{
v.Vector[j] = ((IArrayOf <double>)velocityPre)[j, i];
bg.Vector[j] = ((IArrayOf <double>)bgPre)[j, i];
ba.Vector[j] = ((IArrayOf <double>)baPre)[j, i];
}
for (int j = 0; j < 4; j++)
{
for (int k = 0; k < 4; k++)
{
matrixTcw[j, k] = ((IArrayOf <double>)T_cam_worldPre)[j, k, i];
matrixTbw[j, k] = ((IArrayOf <double>)T_base_worldPre)[j, k, i];
}
}
SE3 Tcw = new SE3(matrixTcw);
SE3 Tbw = new SE3(matrixTbw);
Frame frame = new Frame(((IArrayOf <uint>)idPre)[0, i], ((IArrayOf <double>)timePre)[0, i], Tcw, Tbw, v, bg, ba, ((IArrayOf <double>)scalePre)[0, i]);
frames.Add(frame);
}
return(frames);
}
public Frame(uint id, double time, SE3 Tcw, SE3 Tbw, Vector3 v, Vector3 bg, Vector3 ba, double scale)
{
Id = id;
Time = time;
T_cam_world = Tcw;
T_base_world = Tbw;
Velocity = v;
BiasGyroscope = bg;
BiasAccelerometer = ba;
Scale = scale;
}
/// <summary>
/// Constructor that construct from a json string
/// </summary>
public Annotation(JSONNode node)
{
JSONNode pose = node["pose_information"];
Matrix4x4 m = new Matrix4x4();
m.SetTRS(new Vector3(pose["posX"].AsFloat, pose["posY"].AsFloat, pose["posZ"].AsFloat),
new Quaternion(pose["rotX"].AsFloat, pose["rotY"].AsFloat, pose["rotZ"].AsFloat, pose["rotW"].AsFloat), Vector3.one);
m.m02 = -m.m02;
m.m12 = -m.m12;
m.m20 = -m.m20;
m.m21 = -m.m21;
m.m23 = -m.m23;
Matrix = m;
}
/// <summary>
/// Update all the annotations
/// </summary>
public void Refresh()
{
LCY.Utilities.DestroyChildren(AnnotationsTransform);
LCY.Utilities.DestroyChildren(Rays);
LCY.Utilities.DestroyChildren(Cameras);
RaycastHit hitInfo;
SE3 matrix = Camera.localToWorldMatrix;
// for each annotation
foreach (KeyValuePair <int, Annotation> entry in _Annotations)
{
switch (entry.Value.Type)
{
case Annotation.AnnotationType.TOOL:
ToolAnnotation tool = (ToolAnnotation)entry.Value;
if (Raycast(matrix, tool.Position, out hitInfo))
{
Quaternion localRotation = Quaternion.AngleAxis(tool.Rotation, Vector3.up);
GameObject obj = ObjectFactory.NewTool(AnnotationsTransform, tool.ToolType, localRotation, hitInfo.point, ToolScale);
if (AnnotationAnchor)
{
obj.AddComponent <WorldAnchor>();
}
}
break;
case Annotation.AnnotationType.POLYLINE:
PolylineAnnotation polyline = (PolylineAnnotation)entry.Value;
List <Vector3> positions = new List <Vector3>();
foreach (Vector2 p in polyline.Positions)
{
// only add point when it hits the geometry
if (Raycast(matrix, p, out hitInfo))
{
positions.Add(hitInfo.point);
}
}
if (positions.Count > 0)
{
LineRenderer line = ObjectFactory.NewPolyline(AnnotationsTransform, positions, PolylineColor, PolylineWidth);
if (AnnotationAnchor)
{
line.gameObject.AddComponent <WorldAnchor>();
}
}
break;
}
}
}
public List <KeyFrame> GetKeyFrames()
{
List <KeyFrame> keyFrames = new List <KeyFrame>();
IStructureArray dataStruct = _MatFile["data"].Value as IStructureArray;
IArray timePost = (dataStruct["PostOptimization", 0] as IStructureArray)["time", 0];
IArray idPost = (dataStruct["PostOptimization", 0] as IStructureArray)["id", 0];
IArray T_cam_worldPost = (dataStruct["PostOptimization", 0] as IStructureArray)["T_cam_world", 0];
IArray T_base_worldPost = (dataStruct["PostOptimization", 0] as IStructureArray)["T_base_world", 0];
IArray velocityPost = (dataStruct["PostOptimization", 0] as IStructureArray)["velocity", 0];
IArray bgPost = (dataStruct["PostOptimization", 0] as IStructureArray)["bias_gyroscope", 0];
IArray baPost = (dataStruct["PostOptimization", 0] as IStructureArray)["bias_accelerometer", 0];
IArray kfidPost = (dataStruct["PostOptimization", 0] as IStructureArray)["kfid", 0];
IArray scalePost = (dataStruct["PostOptimization", 0] as IStructureArray)["scale", 0];
IArray fxPost = (dataStruct["PostOptimization", 0] as IStructureArray)["fx", 0];
IArray fyPost = (dataStruct["PostOptimization", 0] as IStructureArray)["fy", 0];
IArray cxPost = (dataStruct["PostOptimization", 0] as IStructureArray)["cx", 0];
IArray cyPost = (dataStruct["PostOptimization", 0] as IStructureArray)["cy", 0];
for (int i = 0; i < timePost.Dimensions[1]; i++)
{
IStructureArray kfstruct = ((dataStruct["PostOptimization", 0] as IStructureArray)["point_cloud", 0] as IStructureArray)["keyframe", i] as IStructureArray;
int points = (kfstruct["u", 0] as IArrayOf <double>).Dimensions[1];
double[,] matrixTcw = new double[4, 4];
double[,] matrixTbw = new double[4, 4];
Vector3 v = new Vector3();
Vector3 bg = new Vector3();
Vector3 ba = new Vector3();
for (int j = 0; j < 3; j++)
{
v.Vector[j] = ((IArrayOf <double>)velocityPost)[j, i];
bg.Vector[j] = ((IArrayOf <double>)bgPost)[j, i];
ba.Vector[j] = ((IArrayOf <double>)baPost)[j, i];
}
for (int j = 0; j < 4; j++)
{
for (int k = 0; k < 4; k++)
{
matrixTcw[j, k] = ((IArrayOf <double>)T_cam_worldPost)[j, k, i];
matrixTbw[j, k] = ((IArrayOf <double>)T_base_worldPost)[j, k, i];
}
}
SE3 Tcw = new SE3(matrixTcw);
SE3 Tbw = new SE3(matrixTbw);
Frame frame = new Frame(((IArrayOf <uint>)idPost)[0, i], ((IArrayOf <double>)timePost)[0, i], Tcw, Tbw, v, bg, ba, ((IArrayOf <double>)scalePost)[0, i]);
KeyFrame keyFrame = new KeyFrame(((IArrayOf <uint>)kfidPost)[0, i], ((IArrayOf <double>)fxPost)[0, i], ((IArrayOf <double>)fyPost)[0, i], ((IArrayOf <double>)cxPost)[0, i], ((IArrayOf <double>)cyPost)[0, i], points, frame);
for (int j = 0; j < points; j++)
{
keyFrame.Points[j] = new Point();
keyFrame.Points[j].U = (kfstruct["u", 0] as IArrayOf <double>)[0, j];
keyFrame.Points[j].V = (kfstruct["v", 0] as IArrayOf <double>)[0, j];
keyFrame.Points[j].InverseDepth = (kfstruct["inverse_depth", 0] as IArrayOf <double>)[0, j];
for (int k = 0; k < 8; k++)
{
keyFrame.Points[j].Colors[k] = (kfstruct["colors", 0] as IArrayOf <byte>)[k, j];
}
}
keyFrames.Add(keyFrame);
}
return(keyFrames);
}
static public string FormatMatrix4x4(SE3 m)
{
return("R:" + FormatQuaternion(m?.Rotation) + " t:" + FormatVector3(m?.Translation));
}
public void Render(VIMatlabImporter matlabImporter, RawDataReader reader, Action <double> progress = null)
{
int count = reader.Count;
double startTime = 0;
int i = 0;
List <KeyFrame> keyFrames = matlabImporter.GetKeyFrames();
List <Frame> frames = matlabImporter.GetFrames();
while (reader.HasNext())
{
i++;
if (i % 100 == 0)
{
progress?.Invoke((double)i / count);
}
Tuple <long, List <Tuple <RawReaderMode, object> > > res = reader.Next();
double time = (double)res.Item1 / (1000 * 1000 * 1000);
if (i == 1)
{
startTime = time;
}
foreach (Tuple <RawReaderMode, object> val in res.Item2)
{
if (val.Item1 == RawReaderMode.Camera0)
{
Mat rawImage = new Mat();
CvInvoke.Imdecode(((Tuple <double, byte[]>)val.Item2).Item2, ImreadModes.Grayscale, rawImage);
List <KeyFrame> kfs = keyFrames.Where(c => c.Frame.Time <= time).ToList();
if (kfs.Count > 0)
{
UpdateLastKeyFrame(kfs.Last(), rawImage, 0.2, 10);
}
foreach (KeyFrame keyFrame in kfs)
{
_SlamModel3D.AddNewKeyFrame(keyFrame);
keyFrames.Remove(keyFrame);
}
List <Frame> fs = frames.Where(c => c.Time <= time).ToList();
foreach (Frame frame in fs)
{
_SlamModel3D.AddNewFrame(frame);
frames.Remove(frame);
}
_SlamModel3D.Render();
SE3 Twc_last = _SlamModel3D.LastTransformation();
MoveCamera(Twc_last.Translation.X, Twc_last.Translation.Y, time - startTime);
WriteFrame(rawImage, RenderViewport());
}
}
}
}
protected void Expand(string FilePath)
{
if (FilePath.Contains('*') || FilePath.Contains('?'))
{
var BasePath = Path.GetDirectoryName(FilePath);
var Recursive = false;
if (BasePath == "")
{
BasePath = ".";
}
foreach (var FileName in Directory.EnumerateFiles(BasePath, Path.GetFileName(FilePath), Recursive ? SearchOption.AllDirectories : SearchOption.TopDirectoryOnly))
{
Expand(FileName);
}
return;
}
var ListToExpand = new List <string>();
//Console.WriteLine("Expanding '{0}'...", FilePath);
using (var _FileStream = File.OpenRead(FilePath))
{
if (_FileStream.Length == 0)
{
//Console.WriteLine("EMPTY: {0}", FilePath);
return;
}
var FileStream = DecompressIfCompressedStream(_FileStream);
var MagicData = FileStream.Slice().ReadBytesUpTo(0x100);
if (false)
{
}
else if (TO8SCEL.IsValid(MagicData))
{
try
{
var To8Scel = new TO8SCEL(FileStream);
foreach (var Entry in To8Scel)
{
var EntryFilePath = FilePath + ".d/" + Entry.Index;
if (Overwrite || !File.Exists(EntryFilePath))
{
Console.WriteLine("{0}", EntryFilePath);
try
{
var EntryStream = DecompressIfCompressedStream(Entry.CompressedStream);
if (EntryStream.Length > 0)
{
EntryStream.CopyToFile(EntryFilePath);
}
}
catch (Exception Exception)
{
ShowException(Exception);
}
}
if (File.Exists(EntryFilePath))
{
ListToExpand.Add(EntryFilePath);
}
}
}
catch (Exception Exception)
{
ShowException(Exception);
}
}
else if (FPS4.IsValid(MagicData))
{
//Console.WriteLine("FPS4");
try
{
var Fps4 = new FPS4(FileStream);
foreach (var Entry in Fps4)
{
var EntryFilePath = FilePath + ".d/" + Entry.Name;
if (Overwrite || !File.Exists(EntryFilePath))
{
Console.WriteLine("{0}", EntryFilePath);
try
{
var EntryStream = DecompressIfCompressedStream(Entry.Open());
if (EntryStream.Length > 0)
{
EntryStream.CopyToFile(EntryFilePath);
}
}
catch (Exception Exception)
{
ShowException(Exception);
}
}
if (File.Exists(EntryFilePath))
{
ListToExpand.Add(EntryFilePath);
}
}
}
catch (Exception Exception)
{
ShowException(Exception);
}
}
else if (TSS.IsValid(MagicData))
{
int RoomId = 0;
try { RoomId = int.Parse(Path.GetFileNameWithoutExtension(FilePath)); }
catch { }
var TxtFile = FilePath + ".txt";
Console.WriteLine("{0}", TxtFile);
if (Overwrite || !File.Exists(TxtFile))
{
var Tss = new TSS().Load(FileStream.Slice());
using (var TxtStream = File.Open(TxtFile, FileMode.Create, FileAccess.Write))
using (var TextWriter = new StreamWriter(TxtStream))
{
try
{
Tss.DumpTexts(TextWriter);
}
catch (Exception Exception)
{
ShowException(Exception);
}
}
}
var ScrFile = FilePath + ".scr";
Console.WriteLine("{0}", ScrFile);
if (Overwrite || !File.Exists(ScrFile))
{
var Tss = new TSS().Load(FileStream.Slice());
using (var TxtStream = File.Open(ScrFile, FileMode.Create, FileAccess.Write))
using (var TextWriter = new StreamWriter(TxtStream))
{
try
{
var ErrorString = ConsoleUtils.CaptureError(() => {
Tss.DumpScript(TextWriter);
});
}
catch (Exception Exception)
{
ShowException(Exception);
}
}
}
}
else if (TO8CHTX.IsValid(MagicData))
{
var Chtx = new TO8CHTX(FileStream);
var TxtFile = FilePath + ".txt";
Console.WriteLine("{0}", TxtFile);
if (Overwrite || !File.Exists(TxtFile))
{
using (var TxtStream = File.Open(TxtFile, FileMode.Create, FileAccess.Write))
using (var TextWriter = new StreamWriter(TxtStream))
{
foreach (var Entry in Chtx.Entries)
{
TextWriter.WriteLine("{0}", Entry.Title);
TextWriter.WriteLine("{0}", Entry.TextOriginal);
TextWriter.WriteLine("{0}", Entry.TextTranslated);
TextWriter.WriteLine("");
}
}
//Chtx.Entries[0].Title
//Console.WriteLine("CHAT!");
}
}
else if (SE3.IsValid(MagicData))
{
var Se3 = new SE3().Load(FileStream);
foreach (var Entry in Se3.Entries)
{
var EntryFullNameXma = FilePath + "." + Entry.Name + ".xma";
var EntryFullNameWav = FilePath + "." + Entry.Name + ".wav";
Console.WriteLine("{0}", EntryFullNameXma);
if (Overwrite || !File.Exists(EntryFullNameXma))
{
Entry.ToXmaWav().CopyToFile(EntryFullNameXma);
}
if (Overwrite || !File.Exists(EntryFullNameWav))
{
using (var WavOut = File.Open(EntryFullNameWav, FileMode.Create, FileAccess.Write))
{
Entry.ToWav(WavOut);
}
}
}
}
else if (TXM.IsValid(MagicData))
{
string BasePath;
string TxmPath;
string TxvPath;
if (Path.GetExtension(FilePath).ToLower() == ".txm")
{
BasePath = Path.GetDirectoryName(FilePath) + "/" + Path.GetFileNameWithoutExtension(FilePath);
TxmPath = BasePath + ".txm";
TxvPath = BasePath + ".txv";
}
else
{
var DirectoryPath = Path.GetDirectoryName(FilePath);
TxmPath = DirectoryPath + "/" + Path.GetFileName(FilePath);
TxvPath = DirectoryPath + "/" + (int.Parse(Path.GetFileName(TxmPath)) + 1);
BasePath = TxmPath;
}
var Txm = TXM.FromTxmTxv(File.OpenRead(TxmPath), File.OpenRead(TxvPath));
/*
* if (Txm.Surface2DEntries.Length > 0 && Txm.Surface3DEntries.Length > 0)
* {
* // 3D and 2D surfaces
* //Console.WriteLine("ERROR 3D and 2D SURFACES! (2D: {0}, 3D: {1})", Txm.Surface2DEntries.Length, Txm.Surface3DEntries.Length);
* }
* else if (Txm.Surface2DEntries.Length > 0)
* {
* // 2D Surfaces
* //Console.WriteLine("2D SURFACES! {0}", Txm.Surface2DEntries.Length);
* }
* else if (Txm.Surface3DEntries.Length > 0)
* {
* // 3D Surfaces
* //Console.WriteLine("3D SURFACES! {0}", Txm.Surface3DEntries.Length);
* }
*/
foreach (var Entry in Txm.Surface2DEntries)
{
var ImagePath = BasePath + "." + Entry.Name + ".png";
if (Overwrite || !File.Exists(ImagePath))
{
try
{
Entry.Bitmap.Save(ImagePath);
}
catch (Exception Exception)
{
ShowException(Exception);
}
}
}
foreach (var Entry in Txm.Surface3DEntries)
{
var ImagePath0 = BasePath + "." + Entry.Name + "." + 0 + ".png";
if (Overwrite || !File.Exists(ImagePath0))
{
try
{
var n = 0;
foreach (var Bitmap in Entry.Bitmaps.Bitmaps)
{
var ImagePath = BasePath + "." + Entry.Name + "." + n + ".png";
Console.WriteLine("{0}", ImagePath);
if (Overwrite || !File.Exists(ImagePath))
{
Bitmap.Save(ImagePath);
}
n++;
}
}
catch (Exception Exception)
{
ShowException(Exception);
}
}
}
}
else
{
}
}
// Expand all the queued stuff
foreach (var Item in ListToExpand)
{
try
{
Expand(Item);
}
catch (Exception Exception)
{
Console.WriteLine(" ERROR: {0}", Verbose ? Exception.ToString() : Exception.Message.ToString());
}
}
}
