69 lines
2.3 KiB
C#
69 lines
2.3 KiB
C#
using System;
|
|
using System.Numerics;
|
|
using System.Runtime.InteropServices;
|
|
|
|
using PointID = System.UInt64;
|
|
|
|
namespace Demo
|
|
{
|
|
public class LivePlotter
|
|
{
|
|
[DllImport("LivePlotter.dll")]
|
|
public static extern bool start(int win_w, int win_h);
|
|
|
|
[DllImport("LivePlotter.dll")]
|
|
public static extern bool stop();
|
|
|
|
[DllImport("LivePlotter.dll")]
|
|
public static extern PointID create_point(float x, float y, float z);
|
|
|
|
[DllImport("LivePlotter.dll")]
|
|
public static extern void set_color(PointID id, float r, float g, float b);
|
|
|
|
[DllImport("LivePlotter.dll")]
|
|
public static extern void set_scale(PointID id, float scale);
|
|
|
|
[DllImport("LivePlotter.dll")]
|
|
public static extern void update_point(PointID id, float x, float y, float z);
|
|
|
|
[DllImport("LivePlotter.dll")]
|
|
public static extern void set_lifetime(PointID id, float new_lifetime_s);
|
|
|
|
[DllImport("LivePlotter.dll")]
|
|
public static extern void clear_point(PointID id);
|
|
|
|
public static void PlotCameraPoses()
|
|
{
|
|
string[] lines = File.ReadAllLines("poses.csv");
|
|
for (int camera_i = 1; camera_i < lines.Length; camera_i++)
|
|
{
|
|
string[] words = lines[camera_i].Split(',');
|
|
float x = float.Parse(words[0]);
|
|
float y = float.Parse(words[1]);
|
|
float z = float.Parse(words[2]);
|
|
float w = float.Parse(words[3]);
|
|
float i = float.Parse(words[4]);
|
|
float j = float.Parse(words[5]);
|
|
float k = float.Parse(words[6]);
|
|
|
|
Pose pose = new(new Quaternion(i, j, k, w), new Vector3(x, y, z));
|
|
for (int m = 0; m < 3; m++)
|
|
{
|
|
for (int n = 0; n < 25; n++)
|
|
{
|
|
Vector3 local = new(0);
|
|
local[m] = n * 10.0f;
|
|
Vector3 global = pose * local;
|
|
PointID id = create_point(global.X, global.Y, global.Z);
|
|
|
|
Vector3 color = new(0);
|
|
color[m] = 1.0f;
|
|
set_color(id, color.X, color.Y, color.Z);
|
|
set_scale(id, 15);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|