protected BillboardSingleImage3D()
{
Positions = new Vector3Collection(6);
Colors = new Color4Collection(6);
TextureCoordinates = new Vector2Collection(6);
MaskColor = Color.Transparent;
}
public BillboardText3D()
{
Positions = new Vector3Collection();
Colors = new Color4Collection();
TextureCoordinates = new Vector2Collection();
this.TextInfo = new List <TextInfo>();
var assembly = Assembly.GetExecutingAssembly();
//Read the texture description
using (var texDescriptionStream = assembly.GetManifestResourceStream("HelixToolkit.Wpf.SharpDX.Textures.arial.fnt"))
{
bmpFont = new BitmapFont();
bmpFont.Load(texDescriptionStream);
}
//Read the texture
using (var texImageStream = assembly.GetManifestResourceStream("HelixToolkit.Wpf.SharpDX.Textures.arial.png"))
{
var image = new BitmapImage();
image.BeginInit();
image.StreamSource = texImageStream;
image.EndInit();
Texture = image;
}
}
public BillboardSingleText3D()
{
Positions = new Vector3Collection();
Colors = new Color4Collection();
TextureCoordinates = new Vector2Collection();
TextInfo = new TextInfo();
}
public static Tuple <Vector3Collection, Color4Collection> BPtoPointCloud(Vector3Collection imageSpace, List <LMData> lmDataList, double angleThreshold = 5, double minCountPercent = 0)
{
if (minCountPercent > 1 || minCountPercent < 0)
{
throw new ArgumentOutOfRangeException(nameof(minCountPercent));
}
Vector3Collection vector3sOut = new Vector3Collection();
Color4Collection color4sOut = new Color4Collection();
if (imageSpace.Count == 0 || lmDataList.Count == 0)
{
var tupleOut1 = new Tuple <Vector3Collection, Color4Collection>(vector3sOut, color4sOut);
return(tupleOut1);
}
int[] counts = new int[imageSpace.Count];
var templmlist = lmDataList.ToArray();
foreach (var lmData in templmlist)
{
if (lmData.Type == LMData.InteractionType.Compton)
{
Parallel.For(0, imageSpace.Count, i =>
{
if (IsEffectedBPPoint(lmData.ScatterLMDataInfos[0].RelativeInteractionPoint3D, lmData.ScatterLMDataInfos[0].InteractionEnergy,
lmData.AbsorberLMDataInfos[0].RelativeInteractionPoint3D, lmData.AbsorberLMDataInfos[0].InteractionEnergy,
imageSpace[i].ToPoint3D(), angleThreshold))
{
counts[i]++;
}
});
}
}
int maxCount = counts.Max();
int minCount = Convert.ToInt32(Math.Round(maxCount * minCountPercent));
if (maxCount < 5)
{
var tupleOut2 = new Tuple <Vector3Collection, Color4Collection>(vector3sOut, color4sOut);
return(tupleOut2);
}
for (int i = 0; i < imageSpace.Count; i++)
{
if (counts[i] > minCount)
{
vector3sOut.Add(imageSpace[i]);
color4sOut.Add(ColorScaleJet(counts[i], minCount, maxCount));
}
}
var tupleOut3 = new Tuple <Vector3Collection, Color4Collection>(vector3sOut, color4sOut);
return(tupleOut3);
}
public BillboardText3D()
{
Positions = new Vector3Collection();
Colors = new Color4Collection();
TextureCoordinates = new Vector2Collection();
TextInfo = new List <TextInfo>();
}
public BillboardSingleText3D()
{
Positions = new Vector3Collection();
Colors = new Color4Collection();
TextureCoordinates = new Vector2Collection();
TextInfo = new TextInfo();
}
public static Color4Collection ScaleGridColors(IEnumerable <HelixToolkit.Wpf.SharpDX.Geometry3D.Line> lines, double min, double max, Enums.colorScales colorScale)
{
var colorList = new Color4Collection();
switchColorScale(colorScale);
foreach (var line in lines)
{
var avg = (line.P0.Z + line.P1.Z) / 2;
if (avg > (float)max)
{
colorList.Add(new Color4(scaleHolder((float)min, (float)max, (float)max)));
colorList.Add(new Color4(scaleHolder((float)min, (float)max, (float)max)));
}
else if (avg < (float)min)
{
colorList.Add(new Color4(scaleHolder((float)min, (float)max, (float)min)));
colorList.Add(new Color4(scaleHolder((float)min, (float)max, (float)min)));
}
else
{
colorList.Add(scaleHolder((float)min, (float)max, (float)avg));
colorList.Add(scaleHolder((float)min, (float)max, (float)avg));
}
}
return(colorList);
}
void DrawBPPointCloudToSLAMPointCloud()
{
Stopwatch sw = new Stopwatch();
sw.Start();
VMStatus = $"Reconing......SLAM! {elapsedTime} ms";
var vc = new Vector3Collection();
var cc = new Color4Collection();
var tempposeVect = new List <double[]>();
var tempColorVect = new List <double[]>();
RealsenseControl.GetReconSLAMPointCloud(ref tempposeVect, ref tempColorVect);
for (int i = 0; i < tempposeVect.Count; i++)
{
vc.Add(new Vector3(Convert.ToSingle(tempposeVect[i][0]), Convert.ToSingle(tempposeVect[i][1]), Convert.ToSingle(tempposeVect[i][2])));
cc.Add(new Color4(Convert.ToSingle(tempColorVect[i][0]), Convert.ToSingle(tempColorVect[i][1]), Convert.ToSingle(tempColorVect[i][2]), 0.5f));
}
SLAMReconPointCloud = new PointGeometry3D()
{
Positions = vc, Colors = cc
};
sw.Stop();
elapsedTime = sw.ElapsedMilliseconds;
VMStatus = "Reconing......SLAM Done!";
}
private void ClearView()
{
// floor plane grid
var Maxreset = 100;
var Minreset = -100;
EffectsManager.Dispose();
EffectsManager = new DefaultEffectsManager();
var points = new PointGeometry3D();
var col = new Color4Collection(); // gradient na kolory
var ptPos = new Vector3Collection(); // pozycje punktów
var ptIdx = new IntCollection(); // indeksy punktów
arrows = lines = new LineBuilder();
points.Positions = ptPos;
points.Indices = ptIdx;
points.Colors = col;
PointsColor = Colors.White; // <- nie pamiętam po co to tutaj ale bez tego nie działa
Points = points;
PointsTransform = new Media3D.TranslateTransform3D(0, 0, Minreset);
CreateGrid(Maxreset, Minreset, Maxreset, Maxreset, Minreset);
Grid = lines.ToLineGeometry3D();
GridColor = new Color4(153 / 255.0f, 204 / 255.0f, 255 / 255.0f, (float)0.3).ToColor();;
GridTransform = new Media3D.TranslateTransform3D(0, 0, Minreset);
// strzałki
CreateArrows(Maxreset, Minreset, Maxreset, Minreset, Maxreset, Minreset);
Arrow = arrows.ToLineGeometry3D();
ArrowColor = new Color4(0, 255 / 255.0f, 255 / 255.0f, (float)0.5).ToColor();;
ArrowTransform = new Media3D.TranslateTransform3D(0, 0, Minreset);
}
public BillboardText3D()
{
Positions = new Vector3Collection();
Colors = new Color4Collection();
TextureCoordinates = new Vector2Collection();
this.TextInfo = new List<TextInfo>();
var assembly = Assembly.GetExecutingAssembly();
//Read the texture description
using(var texDescriptionStream = assembly.GetManifestResourceStream("HelixToolkit.Wpf.SharpDX.Textures.arial.fnt"))
{
bmpFont = new BitmapFont();
bmpFont.Load(texDescriptionStream);
}
//Read the texture
using(var texImageStream = assembly.GetManifestResourceStream("HelixToolkit.Wpf.SharpDX.Textures.arial.png"))
{
var image = new BitmapImage();
image.BeginInit();
image.StreamSource = texImageStream;
image.EndInit();
Texture = image;
}
}
public static MeshGeometry3D Merge(params MeshGeometry3D[] meshes)
{
var positions = new Vector3Collection();
var indices = new IntCollection();
var normals = meshes.All(x => x.Normals != null) ? new Vector3Collection() : null;
var colors = meshes.All(x => x.Colors != null) ? new Color4Collection() : null;
var textureCoods = meshes.All(x => x.TextureCoordinates != null) ? new Vector2Collection() : null;
var tangents = meshes.All(x => x.Tangents != null) ? new Vector3Collection() : null;
var bitangents = meshes.All(x => x.BiTangents != null) ? new Vector3Collection() : null;
int index = 0;
foreach (var part in meshes)
{
positions.AddRange(part.Positions);
indices.AddRange(part.Indices.Select(x => x + index));
index += part.Positions.Count;
}
if (normals != null)
{
normals = new Vector3Collection(meshes.SelectMany(x => x.Normals));
}
if (colors != null)
{
colors = new Color4Collection(meshes.SelectMany(x => x.Colors));
}
if (textureCoods != null)
{
textureCoods = new Vector2Collection(meshes.SelectMany(x => x.TextureCoordinates));
}
if (tangents != null)
{
tangents = new Vector3Collection(meshes.SelectMany(x => x.Tangents));
}
if (bitangents != null)
{
bitangents = new Vector3Collection(meshes.SelectMany(x => x.BiTangents));
}
var mesh = new MeshGeometry3D()
{
Positions = positions,
Indices = indices,
};
mesh.Normals = normals;
mesh.Colors = colors;
mesh.TextureCoordinates = textureCoods;
mesh.Tangents = tangents;
mesh.BiTangents = bitangents;
return(mesh);
}
public BillboardText3D()
{
Positions = new Vector3Collection();
Colors = new Color4Collection();
TextureCoordinates = new Vector2Collection();
TextInfo = new List<TextInfo>();
Initialize();
}
public BillboardSingleText3D(float width, float height)
{
Positions = new Vector3Collection(12);
Colors = new Color4Collection(12);
TextureCoordinates = new Vector2Collection(12);
TextInfo = new TextInfo();
Width = width;
Height = height;
predefinedSize = true;
}
public static (Vector3Collection, Color4Collection) BPtoPointCloud2Pi(Vector3Collection imageSpace, List <LMData> lmDataList, double angleThreshold = 5, double minCountPercent = 0)
{
if (minCountPercent > 1 || minCountPercent < 0)
{
throw new ArgumentOutOfRangeException(nameof(minCountPercent));
}
Vector3Collection vector3sOut = new Vector3Collection();
Color4Collection color4sOut = new Color4Collection();
if (imageSpace.Count == 0 || lmDataList.Count == 0)
{
return(vector3sOut, color4sOut);
}
int[] counts = new int[imageSpace.Count];
foreach (var lmData in lmDataList)
{
if (lmData.Type == LMData.InteractionType.Compton)
{
for (int i = 0; i < imageSpace.Count; ++i)
{
if (IsEffectedBPPoint2Pi(lmData.ScatterLMDataInfo.TransformedInteractionPoint3D, lmData.ScatterLMDataInfo.InteractionEnergy,
lmData.AbsorberLMDataInfo.TransformedInteractionPoint3D, lmData.AbsorberLMDataInfo.InteractionEnergy,
imageSpace[i].ToPoint3D(), lmData.DeviceTransformMatrix, angleThreshold))
{
counts[i]++;
}
}
}
}
int maxCount = counts.Max();
int minCount = Convert.ToInt32(Math.Round(maxCount * minCountPercent));
if (maxCount < 5)
{
return(vector3sOut, color4sOut);
}
for (int i = 0; i < imageSpace.Count; i++)
{
if (counts[i] > minCount)
{
vector3sOut.Add(imageSpace[i]);
color4sOut.Add(ColorScaleJet(counts[i], minCount, maxCount));
}
}
return(vector3sOut, color4sOut);
}
/// <summary>
/// Add axis in positive, 0.1 m
/// </summary>
private void AddAxisPoints()
{
Vector3Collection vector3s = new Vector3Collection();
Color4Collection color4s = new Color4Collection();
vector3s.Add(new SharpDX.Vector3());
color4s.Add(new SharpDX.Color4(0, 0, 0, 1));
for (int i = 1; i < 50; i++)
{
if (i % 10 == 0)
{
vector3s.Add(new SharpDX.Vector3()
{
X = 0.1f * i
});
color4s.Add(new SharpDX.Color4(0, 1, 1, 1));
vector3s.Add(new SharpDX.Vector3()
{
Y = 0.1f * i
});
color4s.Add(new SharpDX.Color4(1, 0, 1, 1));
vector3s.Add(new SharpDX.Vector3()
{
Z = 0.1f * i
});
color4s.Add(new SharpDX.Color4(1, 1, 0, 1));
}
else
{
vector3s.Add(new SharpDX.Vector3()
{
X = 0.1f * i
});
color4s.Add(new SharpDX.Color4(1, 0, 0, 1));
vector3s.Add(new SharpDX.Vector3()
{
Y = 0.1f * i
});
color4s.Add(new SharpDX.Color4(0, 1, 0, 1));
vector3s.Add(new SharpDX.Vector3()
{
Z = 0.1f * i
});
color4s.Add(new SharpDX.Color4(0, 0, 1, 1));
}
}
AxisPoint = new PointGeometry3D()
{
Positions = vector3s, Colors = color4s
};
}
public static IEnumerable <byte> ToEnumerable(this Color4Collection collection)
{
foreach (var c in collection)
{
yield return((byte)(c.Red * 255.0f));
yield return((byte)(c.Green * 255.0f));
yield return((byte)(c.Blue * 255.0f));
yield return((byte)(c.Alpha * 255.0f));
}
}
/// <summary>
/// Create the grid
/// </summary>
private void DrawGrid()
{
Grid = new LineGeometry3D();
var positions = new Vector3Collection();
var indices = new IntCollection();
var colors = new Color4Collection();
for (var i = 0; i < 10; i += 1)
{
for (var j = 0; j < 10; j += 1)
{
DrawGridPatch(positions, indices, colors, -50 + i * 10, -50 + j * 10);
}
}
Grid.Positions = positions;
Grid.Indices = indices;
Grid.Colors = colors;
Axes = new LineGeometry3D();
var axesPositions = new Vector3Collection();
var axesIndices = new IntCollection();
var axesColors = new Color4Collection();
// Draw the coordinate axes
axesPositions.Add(new Vector3());
axesIndices.Add(axesPositions.Count - 1);
axesPositions.Add(new Vector3(50, 0, 0));
axesIndices.Add(axesPositions.Count - 1);
axesColors.Add(SharpDX.Color.Red);
axesColors.Add(SharpDX.Color.Red);
axesPositions.Add(new Vector3());
axesIndices.Add(axesPositions.Count - 1);
axesPositions.Add(new Vector3(0, 5, 0));
axesIndices.Add(axesPositions.Count - 1);
axesColors.Add(SharpDX.Color.Blue);
axesColors.Add(SharpDX.Color.Blue);
axesPositions.Add(new Vector3());
axesIndices.Add(axesPositions.Count - 1);
axesPositions.Add(new Vector3(0, 0, -50));
axesIndices.Add(axesPositions.Count - 1);
axesColors.Add(SharpDX.Color.Green);
axesColors.Add(SharpDX.Color.Green);
Axes.Positions = axesPositions;
Axes.Indices = axesIndices;
Axes.Colors = axesColors;
}
public override object ConvertFrom(ITypeDescriptorContext context, CultureInfo culture, object value)
{
if (value == null)
{
throw GetConvertFromException(value);
}
var source = value as string;
if (source != null)
{
return(Color4Collection.Parse(source));
}
return(base.ConvertFrom(context, culture, value));
}
/// <summary>
/// Sets alpha channel of existing colors to given value, optionally attempts to reset all colors
/// first to original colors from renderpackages.
/// </summary>
/// <param name="geom">Geometry to modify.</param>
/// <param name="alpha">Alpha value to set on geometry. Float between 0 and 1.0.</param>
/// <param name="resetColorsFirst">If true, colors are reset to those stored in the original render packages, before having alpha modified
/// one may want to use this to reset colors from a selected state (blue) back to the original colors before freezing the geo for example. </param>
private static void SetAlpha(GeometryModel3D geom, float alpha, bool resetColorsFirst)
{
if (resetColorsFirst)
{
//reset colors if handled
RequestResetColorsForDynamoGeometryModel?.Invoke(geom.Tag as string);
}
//then modify alpha
var newColors = new Color4Collection(geom.Geometry.Colors.Select(col =>
{
col.Alpha = alpha;
return(col);
}));
geom.Geometry.Colors = newColors;
}
public void SetOspMaterial()
{
if (DiffuseColor != null)
{
this.Material = new PhongMaterial();
Color4Collection colors = new Color4Collection();
for (int i = 0; i < this.Geometry.Positions.Count; i++)
{
colors.Add(DiffuseColor.ToColor4());
}
this.Geometry.Colors = colors;
}
}
public static Color4Collection ToColor4Collection(this byte[] collection)
{
var colors = new Color4Collection();
for (var i = 0; i < collection.Count(); i += 4)
{
var a = collection[i] / 255.0f;
var b = collection[i + 1] / 255.0f;
var c = collection[i + 2] / 255.0f;
var d = collection[i + 3] / 255.0f;
var newColor = new Color4(a, b, c, d);
colors.Add(newColor);
}
return(colors);
}
public void UpdateSLAMPointCloud()
{
int Error = 0;
var pose = new List <HelixToolkit.Wpf.SharpDX.Geometry3D.Line>();
var line = new LineBuilder();
Vector3 previousPose = new Vector3(Convert.ToSingle(poseX), Convert.ToSingle(poseY), Convert.ToSingle(poseZ));
while (IsSLAMOn)
{
var vc = new Vector3Collection();
var id = new IntCollection();
var cc = new Color4Collection();
var poseVect = new List <double[]>();
var colorVect = new List <double[]>();
Vector3 currentPose = new Vector3(Convert.ToSingle(poseX), Convert.ToSingle(poseY), Convert.ToSingle(poseZ));
line.AddLine(previousPose, currentPose);
SLAMPoseInfo = line.ToLineGeometry3D();
previousPose = currentPose;
try
{
RealsenseControl.GetSLAMPointCloud(ref poseVect, ref colorVect, T265ToLACCOffset.X, T265ToLACCOffset.Y, T265ToLACCOffset.Z);
for (int i = 0; i < poseVect.Count; i++)
{
vc.Add(new Vector3(Convert.ToSingle(poseVect[i][0]), Convert.ToSingle(poseVect[i][1]), Convert.ToSingle(poseVect[i][2])));
cc.Add(new Color4(0.1f, 0.1f, 0.1f, 0.5f));
//cc.Add(new Color4(Convert.ToSingle(colorVect[i][0]), Convert.ToSingle(colorVect[i][1]), Convert.ToSingle(colorVect[i][2]), 0.5f));
id.Add(i);
}
SLAMPointCloud = new PointGeometry3D()
{
Positions = vc, Indices = id, Colors = cc
};
}
catch
{
Error++;
Trace.WriteLine("Error Count is " + Error);
}
Thread.Sleep(50);
}
}
private void LoadThreadFunction()
{
PCD wrapper = new PCD();
Points[] rawPoints = wrapper.LoadPointcloud(Location);
var points = new PointGeometry3D();
var col = new Color4Collection();
var ptPos = new Vector3Collection();
var ptIdx = new IntCollection();
var ptNormals = new Vector3Collection();
var numberOfElements = rawPoints.Length;
var additionalTurns = 0;
foreach (var point in rawPoints)
{
ptIdx.Add(ptPos.Count);
ptPos.Add(new Vector3(point.x, point.z, -point.y));
col.Add(new Color4(new Color3(point.r / (float)255, point.g / (float)255, point.b / (float)255)));
ptNormals.Add(new Vector3(0, 1, 0));
}
if ((rawPoints.Length / 3) * 3 != rawPoints.Length)
{
additionalTurns = ((rawPoints.Length / 3 + 1) * 3) - rawPoints.Length;
}
for (int i = 0; i < additionalTurns; i++)
{
ptIdx.Add(ptPos.Count);
ptPos.Add(ptPos[ptPos.Count - 1]);
col.Add(col[col.Count - 1]);
ptNormals.Add(ptNormals[ptNormals.Count - 1]);
}
points.Positions = ptPos;
points.Indices = ptIdx;
points.Colors = col;
//points.Normals = ptNormals;
Data = points;
}
private void LoadThreadFunction()
{
var pt = new Potree(new Uri(Filename));
var points = new PointGeometry3D();
var col = new Color4Collection();
var ptPos = new Vector3Collection();
var ptIdx = new IntCollection();
var j = 0;
foreach (var p in pt.Compilation.Points)
{
ptPos.Add(new Vector3(p.x, p.y, p.z));
ptIdx.Add(j);
col.Add(new Color4(p.r/255f, p.g / 255f, p.b / 255f, p.a / 255f));
j++;
}
var additionalTurns = 0;
if ((pt.Compilation.Points.Count / 3) * 3 != pt.Compilation.Points.Count)
{
additionalTurns = ((pt.Compilation.Points.Count / 3 + 1) * 3) - pt.Compilation.Points.Count;
}
for (int i = 0; i < additionalTurns; i++)
{
ptIdx.Add(ptPos.Count);
ptPos.Add(ptPos[ptPos.Count - 1]);
col.Add(col[col.Count - 1]);
}
points.Positions = ptPos;
points.Indices = ptIdx;
points.Colors = col;
Data = points;
}
private void LoadThreadFunction()
{
var pt = new PotreeNode(new Uri(Location));
var points = new PointGeometry3D();
var col = new Color4Collection();
var ptPos = new Vector3Collection();
var ptIdx = new IntCollection();
var j = 0;
foreach (var p in pt.Compilation.Points)
{
ptPos.Add(new Vector3(p.x, p.y, p.z));
ptIdx.Add(j);
col.Add(new Color4(p.r / 255f, p.g / 255f, p.b / 255f, p.a / 255f));
j++;
}
var additionalTurns = 0;
if ((pt.Compilation.Points.Count / 3) * 3 != pt.Compilation.Points.Count)
{
additionalTurns = ((pt.Compilation.Points.Count / 3 + 1) * 3) - pt.Compilation.Points.Count;
}
for (int i = 0; i < additionalTurns; i++)
{
ptIdx.Add(ptPos.Count);
ptPos.Add(ptPos[ptPos.Count - 1]);
col.Add(col[col.Count - 1]);
}
points.Positions = ptPos;
points.Indices = ptIdx;
points.Colors = col;
Data = points;
}
public static Color4Collection Parse(string source)
{
IFormatProvider formatProvider = CultureInfo.InvariantCulture;
var th = new TokenizerHelper(source, formatProvider);
var resource = new Color4Collection();
Color4 value;
while (th.NextToken())
{
value = new Color4(
Convert.ToSingle(th.GetCurrentToken(), formatProvider),
Convert.ToSingle(th.NextTokenRequired(), formatProvider),
Convert.ToSingle(th.NextTokenRequired(), formatProvider),
Convert.ToSingle(th.NextTokenRequired(), formatProvider));
resource.Add(value);
}
return(resource);
}
public static PointGeometry3D ConvertToPointGeometry3D(Points[] points)
{
var geometry = new PointGeometry3D();
var col = new Color4Collection();
var ptPos = new Vector3Collection();
var ptIdx = new IntCollection();
var ptNormals = new Vector3Collection();
var additionalTurns = 0;
foreach (var point in points)
{
ptIdx.Add(ptPos.Count);
ptPos.Add(new Vector3(point.x, point.y, point.z));
col.Add(new Color4(new Color3(point.r / (float)255, point.g / (float)255, point.b / (float)255)));
ptNormals.Add(new Vector3(0, 1, 0));
}
if ((points.Length / 3) * 3 != points.Length)
{
additionalTurns = ((points.Length / 3 + 1) * 3) - points.Length;
}
for (int i = 0; i < additionalTurns; i++)
{
ptIdx.Add(ptPos.Count);
ptPos.Add(ptPos[ptPos.Count - 1]);
col.Add(col[col.Count - 1]);
ptNormals.Add(ptNormals[ptNormals.Count - 1]);
}
geometry.Positions = ptPos;
geometry.Indices = ptIdx;
geometry.Colors = col;
return(geometry);
}
public BillboardText3D()
{
Positions = new Vector3Collection();
Colors = new Color4Collection();
TextureCoordinates = new Vector2Collection();
this.TextInfo = new List <TextInfo>();
var assembly = Assembly.GetExecutingAssembly();
var texDescriptionFilePath = Path.GetTempFileName();
var texImageFilePath = Path.GetTempFileName();
//Read the texture description
var texDescriptionStream = assembly.GetManifestResourceStream("HelixToolkit.Wpf.SharpDX.Textures.arial.fnt");
using (var fileStream = File.Create(texDescriptionFilePath))
{
texDescriptionStream.CopyTo(fileStream);
}
bmpFont = BitmapFontLoader.LoadFontFromFile(texDescriptionFilePath);
//Read the texture
var texImageStream = assembly.GetManifestResourceStream("HelixToolkit.Wpf.SharpDX.Textures.arial.png");
using (var fileStream = File.Create(texImageFilePath))
{
texImageStream.CopyTo(fileStream);
}
Texture = new BitmapImage(new Uri(texImageFilePath));
//Cleanup the temp files
if (File.Exists(texDescriptionFilePath))
{
File.Delete(texDescriptionFilePath);
}
}
public BillboardText3D()
{
Positions = new Vector3Collection();
Colors = new Color4Collection();
TextureCoordinates = new Vector2Collection();
this.TextInfo = new List<TextInfo>();
var assembly = Assembly.GetExecutingAssembly();
var texDescriptionFilePath = Path.GetTempFileName();
var texImageFilePath = Path.GetTempFileName();
//Read the texture description
var texDescriptionStream = assembly.GetManifestResourceStream("HelixToolkit.Wpf.SharpDX.Textures.arial.fnt");
using (var fileStream = File.Create(texDescriptionFilePath))
{
texDescriptionStream.CopyTo(fileStream);
}
bmpFont = BitmapFontLoader.LoadFontFromFile(texDescriptionFilePath);
//Read the texture
var texImageStream = assembly.GetManifestResourceStream("HelixToolkit.Wpf.SharpDX.Textures.arial.png");
using (var fileStream = File.Create(texImageFilePath))
{
texImageStream.CopyTo(fileStream);
}
Texture = new BitmapImage(new Uri(texImageFilePath));
//Cleanup the temp files
if (File.Exists(texDescriptionFilePath))
{
File.Delete(texDescriptionFilePath);
}
}
public void UpdateRealTimePointCloud()
{
int error = 0;
while (IsRealsensePipelineOn)
{
var vc = new Vector3Collection();
var id = new IntCollection();
var cc = new Color4Collection();
var poseVect = new List <double[]>();
var colorVect = new List <double[]>();
//Debug.WriteLine("Start to get Reatime Data");
try
{
RealsenseControl.GetRealTimePointCloud(ref poseVect, ref colorVect, T265ToLACCOffset.X, T265ToLACCOffset.Y, T265ToLACCOffset.Z);
for (int i = 0; i < poseVect.Count; i++)
{
vc.Add(new Vector3(Convert.ToSingle(poseVect[i][0]), Convert.ToSingle(poseVect[i][1]), Convert.ToSingle(poseVect[i][2])));
cc.Add(new Color4(Convert.ToSingle(colorVect[i][0]), Convert.ToSingle(colorVect[i][1]), Convert.ToSingle(colorVect[i][2]), 0.8f));
//id.Add(i);
}
RTPointCloud = new PointGeometry3D()
{
Positions = vc, Colors = cc
};
}
catch
{
error++;
Trace.WriteLine("Error Count is " + error);
}
}
}
/// <summary>
///
/// </summary>
/// <param name="mesh"></param>
/// <param name="which"></param>
/// <param name="color"></param>
/// <param name="label"></param>
/// <param name="labelColor"></param>
protected void UpdateAxisColor(Geometry3D mesh, int which, Color4 color, string label, Color4 labelColor)
{
var labelText = axisBillboard.Geometry as BillboardText3D;
switch (which)
{
case 0:
labelText.TextInfo[which] = new TextInfo(label, new Vector3(arrowSize + 1.5f, 0, 0))
{
Foreground = labelColor, Scale = 0.5f
};
break;
case 1:
labelText.TextInfo[which] = new TextInfo(label, new Vector3(0, arrowSize + 1.5f, 0))
{
Foreground = labelColor, Scale = 0.5f
};
break;
case 2:
labelText.TextInfo[which] = new TextInfo(label, new Vector3(0, 0, arrowSize + 1.5f))
{
Foreground = labelColor, Scale = 0.5f
};
break;
}
int segment = mesh.Positions.Count / 3;
var colors = new Color4Collection(mesh.Colors == null ? Enumerable.Repeat <Color4>(Color.Black, mesh.Positions.Count) : mesh.Colors);
for (int i = segment * which; i < segment * (which + 1); ++i)
{
colors[i] = color;
}
mesh.Colors = colors;
}
public static Color4Collection ScalePointColors(Vector3Collection points, double actualMin, double actualMax, double newMin, double newMax, Enums.colorScales colorScale)
{
var temp = new Color4Collection();
switchColorScale(colorScale);
foreach (var item in points)
{
if (item.Z > (float)newMax)
{
temp.Add(new Color4(VisualizationTools.scaleHolder((float)actualMin, (float)newMax, (float)newMax)));
}
else if (item.Z < (float)newMin)
{
temp.Add(new Color4(VisualizationTools.scaleHolder((float)newMin, (float)actualMax, (float)newMin)));
}
else
{
temp.Add(new Color4(VisualizationTools.scaleHolder((float)newMin, (float)newMax, item.Z)));
}
}
return(temp);
}
public static MeshGeometry3D Merge(params MeshGeometry3D[] meshes)
{
var positions = new Vector3Collection();
var indices = new IntCollection();
var normals = meshes.All(x => x.Normals != null) ? new Vector3Collection() : null;
var colors = meshes.All(x => x.Colors != null) ? new Color4Collection() : null;
var textureCoods = meshes.All(x => x.TextureCoordinates != null) ? new Vector2Collection() : null;
var tangents = meshes.All(x => x.Tangents != null) ? new Vector3Collection() : null;
var bitangents = meshes.All(x => x.BiTangents != null) ? new Vector3Collection() : null;
int index = 0;
foreach (var part in meshes)
{
for (int i = 0; i < part.Positions.Count; i++)
{
positions.Add(part.Positions[i]);
}
for (int i = 0; i < part.Indices.Count; i++)
{
indices.Add(index + part.Indices[i]);
}
index += part.Indices.Count;
}
if (normals != null)
{
normals = new Vector3Collection(meshes.SelectMany(x => x.Normals));
}
if (colors != null)
{
colors = new Color4Collection(meshes.SelectMany(x => x.Colors));
}
if (textureCoods != null)
{
textureCoods = new Vector2Collection(meshes.SelectMany(x => x.TextureCoordinates));
}
if (tangents != null)
{
tangents = new Vector3Collection(meshes.SelectMany(x => x.Tangents));
}
if (bitangents != null)
{
bitangents = new Vector3Collection(meshes.SelectMany(x => x.BiTangents));
}
var mesh = new MeshGeometry3D()
{
Positions = positions,
Indices = indices,
};
mesh.Normals = normals;
mesh.Colors = colors;
mesh.TextureCoordinates = textureCoods;
mesh.Tangents = tangents;
mesh.BiTangents = bitangents;
return mesh;
}
/// <summary>
/// Sets all colors on Geometry to a single color.
/// </summary>
/// <param name="geom">Geometry to modify.</param>
/// <param name="color">Color to set geometry to.</param>
private static void SetAllColors(GeometryModel3D geom, Color4 color)
{
var newColorCollection = new Color4Collection(Enumerable.Repeat(color, geom.Geometry.Colors.Count));
geom.Geometry.Colors = newColorCollection;
}
private static void DrawGridPatch(
Vector3Collection positions, IntCollection indices, Color4Collection colors, int startX, int startY)
{
var c1 = (Color)ColorConverter.ConvertFromString("#c5d1d8");
c1.Clamp();
var c2 = (Color)ColorConverter.ConvertFromString("#ddeaf2");
c2.Clamp();
var darkGridColor = new Color4(new Vector4(c1.ScR,c1.ScG ,c1.ScB, 1));
var lightGridColor = new Color4(new Vector4(c2.ScR, c2.ScG, c2.ScB, 1));
const int size = 10;
for (var x = startX; x <= startX + size; x++)
{
if (x == 0 && startY < 0) continue;
var v = new Vector3(x, -.001f, startY);
positions.Add(v);
indices.Add(positions.Count - 1);
positions.Add(new Vector3(x, -.001f, startY + size));
indices.Add(positions.Count - 1);
if (x % 5 == 0)
{
colors.Add(darkGridColor);
colors.Add(darkGridColor);
}
else
{
colors.Add(lightGridColor);
colors.Add(lightGridColor);
}
}
for (var y = startY; y <= startY + size; y++)
{
if (y == 0 && startX >= 0) continue;
positions.Add(new Vector3(startX, -.001f, y));
indices.Add(positions.Count - 1);
positions.Add(new Vector3(startX + size, -.001f, y));
indices.Add(positions.Count - 1);
if (y % 5 == 0)
{
colors.Add(darkGridColor);
colors.Add(darkGridColor);
}
else
{
colors.Add(lightGridColor);
colors.Add(lightGridColor);
}
}
}
/// <summary>
/// Create the grid
/// </summary>
private void DrawGrid()
{
Grid = new LineGeometry3D();
var positions = new Vector3Collection();
var indices = new IntCollection();
var colors = new Color4Collection();
for(var i= 0; i < 10; i += 1)
{
for (var j = 0; j < 10; j += 1)
{
DrawGridPatch(positions, indices, colors, -50 + i * 10, -50 + j * 10);
}
}
Grid.Positions = positions;
Grid.Indices = indices;
Grid.Colors = colors;
Axes = new LineGeometry3D();
var axesPositions = new Vector3Collection();
var axesIndices = new IntCollection();
var axesColors = new Color4Collection();
// Draw the coordinate axes
axesPositions.Add(new Vector3());
axesIndices.Add(axesPositions.Count - 1);
axesPositions.Add(new Vector3(50, 0, 0));
axesIndices.Add(axesPositions.Count - 1);
axesColors.Add(SharpDX.Color.Red);
axesColors.Add(SharpDX.Color.Red);
axesPositions.Add(new Vector3());
axesIndices.Add(axesPositions.Count - 1);
axesPositions.Add(new Vector3(0, 5, 0));
axesIndices.Add(axesPositions.Count - 1);
axesColors.Add(SharpDX.Color.Blue);
axesColors.Add(SharpDX.Color.Blue);
axesPositions.Add(new Vector3());
axesIndices.Add(axesPositions.Count - 1);
axesPositions.Add(new Vector3(0, 0, -50));
axesIndices.Add(axesPositions.Count - 1);
axesColors.Add(SharpDX.Color.Green);
axesColors.Add(SharpDX.Color.Green);
Axes.Positions = axesPositions;
Axes.Indices = axesIndices;
Axes.Colors = axesColors;
}
public static PointGeometry3D ConvertToPointGeometry3D(Points[] points)
{
var geometry = new PointGeometry3D();
var col = new Color4Collection();
var ptPos = new Vector3Collection();
var ptIdx = new IntCollection();
var ptNormals = new Vector3Collection();
var additionalTurns = 0;
foreach (var point in points)
{
ptIdx.Add(ptPos.Count);
ptPos.Add(new Vector3(point.x, point.y, point.z));
col.Add(new Color4(new Color3(point.r / (float)255, point.g / (float)255, point.b / (float)255)));
ptNormals.Add(new Vector3(0, 1, 0));
}
if ((points.Length / 3) * 3 != points.Length)
{
additionalTurns = ((points.Length / 3 + 1) * 3) - points.Length;
}
for (int i = 0; i < additionalTurns; i++)
{
ptIdx.Add(ptPos.Count);
ptPos.Add(ptPos[ptPos.Count - 1]);
col.Add(col[col.Count - 1]);
ptNormals.Add(ptNormals[ptNormals.Count - 1]);
}
geometry.Positions = ptPos;
geometry.Indices = ptIdx;
geometry.Colors = col;
return geometry;
}
public void UpdateSLAMPointCloud()
{
var line = new LineBuilder();
Vector3 previousPose = new Vector3(Convert.ToSingle(systemPoseX), Convert.ToSingle(systemPoseY), Convert.ToSingle(systemPoseZ));
var poseVect = new List <double[]>();
var colorVect = new List <double[]>();
Point3D lineVect = new Point3D(0, 0, 0.3);
while (IsSLAMOn)
{
Thread.Sleep(500);
var vc = new Vector3Collection();
var cc = new Color4Collection();
poseVect = new List <double[]>();
colorVect = new List <double[]>();
Vector3 currentPose = new Vector3(Convert.ToSingle(systemPoseX), Convert.ToSingle(systemPoseY), Convert.ToSingle(systemPoseZ));
line.AddLine(previousPose, currentPose);
SLAMPoseInfo = line.ToLineGeometry3D();
previousPose = currentPose;
RealsenseControl.GetSLAMPointCloud(ref poseVect, ref colorVect);
for (int i = 0; i < poseVect.Count; i++)
{
vc.Add(new Vector3(Convert.ToSingle(poseVect[i][0]), Convert.ToSingle(poseVect[i][1]), Convert.ToSingle(poseVect[i][2])));
//cc.Add(new Color4(0.1f, 0.1f, 0.1f, 0.5f));
cc.Add(new Color4(Convert.ToSingle(colorVect[i][0]), Convert.ToSingle(colorVect[i][1]), Convert.ToSingle(colorVect[i][2]), 0.5f));
//id.Add(i);
}
for (int i = 0; i < 10; ++i)
{
var pointx = new Point3D(0.1 * i, 0, 0);
var pointy = new Point3D(0, 0.1 * i, 0);
var pointz = new Point3D(0, 0, 0.1 * i);
vc.Add(new Vector3(Convert.ToSingle(pointx.X), Convert.ToSingle(pointx.Y), Convert.ToSingle(pointx.Z)));
cc.Add(new Color4(1f, 0f, 0f, 1f));
vc.Add(new Vector3(Convert.ToSingle(pointy.X), Convert.ToSingle(pointy.Y), Convert.ToSingle(pointy.Z)));
cc.Add(new Color4(0f, 1f, 0f, 1f));
vc.Add(new Vector3(Convert.ToSingle(pointz.X), Convert.ToSingle(pointz.Y), Convert.ToSingle(pointz.Z)));
cc.Add(new Color4(0f, 0f, 1f, 1f));
}
for (int i = 0; i < 10; ++i)
{
var pointx = currentSystemTranformation.Transform(new Point3D(0.1 * i, 0, 0));
var pointy = currentSystemTranformation.Transform(new Point3D(0, 0.1 * i, 0));
var pointz = currentSystemTranformation.Transform(new Point3D(0, 0, 0.1 * i));
vc.Add(new Vector3(Convert.ToSingle(pointx.X), Convert.ToSingle(pointx.Y), Convert.ToSingle(pointx.Z)));
cc.Add(new Color4(1f, 0f, 0f, 1f));
vc.Add(new Vector3(Convert.ToSingle(pointy.X), Convert.ToSingle(pointy.Y), Convert.ToSingle(pointy.Z)));
cc.Add(new Color4(0f, 1f, 0f, 1f));
vc.Add(new Vector3(Convert.ToSingle(pointz.X), Convert.ToSingle(pointz.Y), Convert.ToSingle(pointz.Z)));
cc.Add(new Color4(0f, 0f, 1f, 1f));
}
SLAMPointCloud = new PointGeometry3D()
{
Positions = vc, Colors = cc
};
SLAMVector3s = vc;
}
Debug.WriteLine("SLAM Point Cloud Count is " + poseVect.Count);
}
public CameraModel3D()
{
var b1 = new MeshBuilder();
b1.AddBox(new Vector3(), 1f, 1f, 1.2f, BoxFaces.All);
var body = new MeshGeometryModel3D()
{
CullMode = CullMode.Back
};
body.Geometry = b1.ToMeshGeometry3D();
body.Material = new DiffuseMaterial()
{
DiffuseColor = Color.Gray
};
this.Children.Add(body);
b1 = new MeshBuilder();
b1.AddCone(new Vector3(0, 0, -1.2f), new Vector3(0, 0f, 0), 0.4f, true, 12);
var lens = new MeshGeometryModel3D()
{
CullMode = CullMode.Back
};
lens.Geometry = b1.ToMeshGeometry3D();
lens.Material = new DiffuseMaterial()
{
DiffuseColor = Color.Yellow
};
this.Children.Add(lens);
var builder = new LineBuilder();
builder.AddLine(Vector3.Zero, new Vector3(2, 0, 0));
builder.AddLine(Vector3.Zero, new Vector3(0, 2, 0));
builder.AddLine(Vector3.Zero, new Vector3(0, 0, -2));
var mesh = builder.ToLineGeometry3D();
var arrowMeshModel = new LineGeometryModel3D
{
Geometry = mesh,
Color = System.Windows.Media.Colors.White,
IsHitTestVisible = false
};
int segment = mesh.Positions.Count / 3;
var colors = new Color4Collection(Enumerable.Repeat <Color4>(Color.Black, mesh.Positions.Count));
int i = 0;
for (; i < segment; ++i)
{
colors[i] = Color.Red;
}
for (; i < segment * 2; ++i)
{
colors[i] = Color.Green;
}
for (; i < segment * 3; ++i)
{
colors[i] = Color.Blue;
}
mesh.Colors = colors;
this.Children.Add(arrowMeshModel);
SceneNode.TransformChanged += SceneNode_OnTransformChanged;
}
public static Color4Collection ToColor4Collection(this byte[] collection)
{
var colors = new Color4Collection();
for(var i = 0; i<collection.Count(); i+=4)
{
var a = collection[i]/255.0f;
var b = collection[i + 1]/255.0f;
var c = collection[i + 2]/255.0f;
var d = collection[i + 3]/255.0f;
var newColor = new Color4(a, b, c, d);
colors.Add(newColor);
}
return colors;
}
private void LoadThreadFunction()
{
PCD wrapper = new PCD();
Points[] rawPoints = wrapper.LoadPointcloud(Filename);
var points = new PointGeometry3D();
var col = new Color4Collection();
var ptPos = new Vector3Collection();
var ptIdx = new IntCollection();
var ptNormals = new Vector3Collection();
var numberOfElements = rawPoints.Length;
var additionalTurns = 0;
foreach (var point in rawPoints)
{
ptIdx.Add(ptPos.Count);
ptPos.Add(new Vector3(point.x, point.z, -point.y));
col.Add(new Color4(new Color3(point.r / (float)255, point.g / (float)255, point.b / (float)255)));
ptNormals.Add(new Vector3(0, 1, 0));
}
if ((rawPoints.Length / 3) * 3 != rawPoints.Length)
{
additionalTurns = ((rawPoints.Length / 3 + 1) * 3) - rawPoints.Length;
}
for (int i = 0; i < additionalTurns; i++)
{
ptIdx.Add(ptPos.Count);
ptPos.Add(ptPos[ptPos.Count-1]);
col.Add(col[col.Count - 1]);
ptNormals.Add(ptNormals[ptNormals.Count - 1]);
}
points.Positions = ptPos;
points.Indices = ptIdx;
points.Colors = col;
//points.Normals = ptNormals;
Data = points;
}
