AForge.Math.Vector3 getLEDLocCenter(Rectangle ledpos)
{
AForge.Math.Vector3 ret = new AForge.Math.Vector3();
ret.X = (ledpos.X + ledpos.Width / 2.0f);
ret.Y = (ledpos.Y + ledpos.Height / 2.0f);
return(ret);
}
public void setModelPoints(Vector3[] modelPoints) {
modelPoint1xBox.Text = modelPoints[0].X.ToString();
modelPoint1yBox.Text = modelPoints[0].Y.ToString();
modelPoint1zBox.Text = modelPoints[0].Z.ToString();
modelPoint2xBox.Text = modelPoints[1].X.ToString();
modelPoint2yBox.Text = modelPoints[1].Y.ToString();
modelPoint2zBox.Text = modelPoints[1].Z.ToString();
modelPoint3xBox.Text = modelPoints[2].X.ToString();
modelPoint3yBox.Text = modelPoints[2].Y.ToString();
modelPoint3zBox.Text = modelPoints[2].Z.ToString();
modelPoint4xBox.Text = modelPoints[3].X.ToString();
modelPoint4yBox.Text = modelPoints[3].Y.ToString();
modelPoint4zBox.Text = modelPoints[3].Z.ToString();
}
void theCamera_NewFrame(object sender, AForge.Video.NewFrameEventArgs eventArgs)
{
Bitmap theFrame = eventArgs.Frame;
AForge.Imaging.Filters.ColorFiltering colorFilter = new AForge.Imaging.Filters.ColorFiltering(new AForge.IntRange(200, 255),
new AForge.IntRange(200, 255),
new AForge.IntRange(200, 255));
colorFilter.ApplyInPlace(theFrame);
AForge.Imaging.BlobCounter blobCounter = new AForge.Imaging.BlobCounter(theFrame);
var ledLOCs = blobCounter.GetObjectsRectangles();
string ledpos = "not there";
if (ledLOCs.Length > 0)
{
//should probably find out which one is actually the LED... later though.
var pos = getLEDLocCenter(ledLOCs[0]);
var centerPos = new AForge.Math.Vector3(theFrame.Width / 2, theFrame.Height / 2, 0);
var ledVector = pos - centerPos;
ledVector.Normalize();
var ledAngle = Math.Acos(ledVector.X);
if (ledVector.Y < 0)
{
ledAngle = -ledAngle;
}
double deltaLEDAngle = 0;
if (ledAngleInit)
{
deltaLEDAngle = ledAngle - lastLEDAngle;
if (deltaLEDAngle > Math.PI * 2)
{
deltaLEDAngle -= Math.PI * 2;
}
if (deltaLEDAngle < -Math.PI * 2)
{
deltaLEDAngle += Math.PI * 2;
}
lastLEDAngle = ledAngle;
}
else
{
ledAngleInit = true;
lastLEDAngle = ledAngle;
}
deltaLEDAngle = ledAngle * 180 / Math.PI;
bool canTransmit = false;
vrconnectMutex.WaitOne();
if (VRapp != null)
{
canTransmit = true;
}
vrconnectMutex.ReleaseMutex();
if (canTransmit)
{
byte [] deltaAngleBytes = BitConverter.GetBytes(deltaLEDAngle); //note: in degrees per frame
try
{
if (VRapp.Connected)
{
VRapp.GetStream().Write(deltaAngleBytes, 0, deltaAngleBytes.Length);
}
}
catch
{
VRapp = null;
canTransmit = false;
}
}
double displayLEDAngle = deltaLEDAngle;
ledpos = displayLEDAngle.ToString();
var triVec = ledPoints(ledLOCs);
if (triVec != new Vector4(0, 0, 0, 0))
{
ledpos = triVec.ToString();
Graphics g = Graphics.FromImage(theFrame);
g.DrawLine(Pens.Red, triVec.X, triVec.Y, triVec.Z, triVec.W);
g.Flush();
g.Dispose();
}
//if we're connected to the VR app, this is where we send the delta angle.
}
updateTheFrame(theFrame, ledpos);
}
