private Vector3[] GenerateVertices()
{
int smoothingPasses = (noise.overtones * smoothings) + 1;
HeightMap map = new HeightMap(SmoothFilter.ComputeStartSize(size + smoothingPasses, smoothingPasses));
SmoothFilter smooth = new SmoothFilter();
noise.ClearOvertoneFilters();
for (int i = 0; i != smoothings; ++i)
{
noise.AddOvertoneFilter(smooth);
}
map = smooth.Filter(weathering.Filter(typeFilter.Filter(noise.Filter(map))));
int dif = map.Size - size;
int offset = dif >> 1;
Vector3[] vertices = map.ToVertices(offset, offset, size);
ClampToOrigin(vertices);
return(vertices);
}
/// <summary>
/// The thread funktion to poll the telemetry data and send TelemetryUpdated events.
/// </summary>
private void Run()
{
isStopped = false;
WFSTAPI lastTelemetryData = new WFSTAPI();
lastTelemetryData.Reset();
Matrix4x4 lastTransform = Matrix4x4.Identity;
bool lastFrameValid = false;
Vector3 lastVelocity = Vector3.Zero;
float lastYaw = 0.0f;
Stopwatch sw = new Stopwatch();
sw.Start();
NestedSmooth accXSmooth = new NestedSmooth(3, 6, 0.5f);
NestedSmooth accYSmooth = new NestedSmooth(3, 6, 0.5f);
NestedSmooth accZSmooth = new NestedSmooth(3, 6, 0.5f);
KalmanFilter velXFilter = new KalmanFilter(1, 1, 0.02f, 1, 0.02f, 0.0f);
KalmanFilter velZFilter = new KalmanFilter(1, 1, 0.02f, 1, 0.02f, 0.0f);
NoiseFilter velXSmooth = new NoiseFilter(6, 0.5f);
NoiseFilter velZSmooth = new NoiseFilter(6, 0.5f);
KalmanFilter yawRateFilter = new KalmanFilter(1, 1, 0.02f, 1, 0.02f, 0.0f);
NoiseFilter yawRateSmooth = new NoiseFilter(6, 0.5f);
NoiseFilter pitchFilter = new NoiseFilter(3);
NoiseFilter rollFilter = new NoiseFilter(3);
NoiseFilter yawFilter = new NoiseFilter(3);
KalmanFilter posXFilter = new KalmanFilter(1, 1, 0.02f, 1, 0.1f, 0.0f);
KalmanFilter posYFilter = new KalmanFilter(1, 1, 0.02f, 1, 0.1f, 0.0f);
KalmanFilter posZFilter = new KalmanFilter(1, 1, 0.02f, 1, 0.1f, 0.0f);
NestedSmooth posXSmooth = new NestedSmooth(12, 6, 0.5f);
NestedSmooth posYSmooth = new NestedSmooth(12, 6, 0.5f);
NestedSmooth posZSmooth = new NestedSmooth(12, 6, 0.5f);
NoiseFilter slipAngleSmooth = new NoiseFilter(6, 0.25f);
ProcessMemoryReader reader = new ProcessMemoryReader();
reader.ReadProcess = wfstProcess;
uint readSize = 4 * 4 * 4;
byte[] readBuffer = new byte[readSize];
reader.OpenProcess();
while (!isStopped)
{
try
{
float dt = (float)sw.ElapsedMilliseconds / 1000.0f;
int byteReadSize;
reader.ReadProcessMemory((IntPtr)memoryAddress, readSize, out byteReadSize, readBuffer);
if (byteReadSize == 0)
{
continue;
}
float[] floats = new float[4 * 4];
Buffer.BlockCopy(readBuffer, 0, floats, 0, readBuffer.Length);
Matrix4x4 transform = new Matrix4x4(floats[0], floats[1], floats[2], floats[3]
, floats[4], floats[5], floats[6], floats[7]
, floats[8], floats[9], floats[10], floats[11]
, floats[12], floats[13], floats[14], floats[15]);
Vector3 rht = new Vector3(transform.M11, transform.M12, transform.M13);
Vector3 up = new Vector3(transform.M21, transform.M22, transform.M23);
Vector3 fwd = new Vector3(transform.M31, transform.M32, transform.M33);
float rhtMag = rht.Length();
float upMag = up.Length();
float fwdMag = fwd.Length();
//reading garbage
if (rhtMag < 0.9f || upMag < 0.9f || fwdMag < 0.9f)
{
IsConnected = false;
IsRunning = false;
TelemetryLost();
break;
}
if (!lastFrameValid)
{
lastTransform = transform;
lastFrameValid = true;
lastVelocity = Vector3.Zero;
lastYaw = 0.0f;
continue;
}
WFSTAPI telemetryData = new WFSTAPI();
if (dt <= 0)
{
dt = 1.0f;
}
Vector3 worldVelocity = (transform.Translation - lastTransform.Translation) / dt;
lastTransform = transform;
Matrix4x4 rotation = new Matrix4x4();
rotation = transform;
rotation.M41 = 0.0f;
rotation.M42 = 0.0f;
rotation.M43 = 0.0f;
Matrix4x4 rotInv = new Matrix4x4();
Matrix4x4.Invert(rotation, out rotInv);
Vector3 localVelocity = Vector3.Transform(worldVelocity, rotInv);
telemetryData.velX = worldVelocity.X;
telemetryData.velZ = worldVelocity.Z;
Vector3 localAcceleration = localVelocity - lastVelocity;
lastVelocity = localVelocity;
telemetryData.accX = localAcceleration.X * 10.0f;
telemetryData.accY = localAcceleration.Y * 100.0f;
telemetryData.accZ = localAcceleration.Z * 10.0f;
float pitch = (float)Math.Asin(-fwd.Y);
float yaw = (float)Math.Atan2(fwd.X, fwd.Z);
float roll = 0.0f;
Vector3 rhtPlane = rht;
rhtPlane.Y = 0;
rhtPlane = Vector3.Normalize(rhtPlane);
if (rhtPlane.Length() <= float.Epsilon)
{
roll = -(float)(Math.Sign(rht.Y) * Math.PI * 0.5f);
}
else
{
roll = -(float)Math.Asin(Vector3.Dot(up, rhtPlane));
}
telemetryData.pitchPos = pitch;
telemetryData.yawPos = yaw;
telemetryData.rollPos = roll;
telemetryData.yawRate = CalculateAngularChange(lastYaw, yaw) * (180.0f / (float)Math.PI);
lastYaw = yaw;
// otherwise we are connected
IsConnected = true;
if (IsConnected)
{
IsRunning = true;
WFSTAPI telemetryToSend = new WFSTAPI();
telemetryToSend.Reset();
telemetryToSend.CopyFields(telemetryData);
telemetryToSend.accX = accXSmooth.Filter(telemetryData.accX);
telemetryToSend.accY = accYSmooth.Filter(telemetryData.accY);
telemetryToSend.accZ = accZSmooth.Filter(telemetryData.accZ);
telemetryToSend.pitchPos = pitchFilter.Filter(telemetryData.pitchPos);
telemetryToSend.rollPos = rollFilter.Filter(telemetryData.rollPos);
telemetryToSend.yawPos = yawFilter.Filter(telemetryData.yawPos);
telemetryToSend.velX = velXSmooth.Filter(velXFilter.Filter(telemetryData.velX));
telemetryToSend.velZ = velZSmooth.Filter(velZFilter.Filter(telemetryData.velZ));
telemetryToSend.yawRate = yawRateSmooth.Filter(yawRateFilter.Filter(telemetryData.yawRate));
telemetryToSend.yawAcc = slipAngleSmooth.Filter(telemetryToSend.CalculateSlipAngle());
sw.Restart();
TelemetryEventArgs args = new TelemetryEventArgs(
new WFSTTelemetryInfo(telemetryToSend, lastTelemetryData));
RaiseEvent(OnTelemetryUpdate, args);
lastTelemetryData = telemetryToSend;
Thread.Sleep(1000 / 100);
}
else if (sw.ElapsedMilliseconds > 500)
{
IsRunning = false;
}
}
catch (Exception e)
{
LogError("WFSTTelemetryProvider Exception while processing data", e);
IsConnected = false;
IsRunning = false;
Thread.Sleep(1000);
}
}
IsConnected = false;
IsRunning = false;
reader.CloseHandle();
}
/// <summary>
/// The thread funktion to poll the telemetry data and send TelemetryUpdated events.
/// </summary>
private void Run()
{
isStopped = false;
DIRT5API lastTelemetryData = new DIRT5API();
lastTelemetryData.Reset();
Matrix4x4 lastTransform = Matrix4x4.Identity;
bool lastFrameValid = false;
Vector3 lastVelocity = Vector3.Zero;
float lastYaw = 0.0f;
Stopwatch sw = new Stopwatch();
sw.Start();
NestedSmooth accXSmooth = new NestedSmooth(3, 6, 0.5f);
NestedSmooth accYSmooth = new NestedSmooth(3, 6, 0.5f);
NestedSmooth accZSmooth = new NestedSmooth(3, 6, 0.5f);
KalmanFilter velXFilter = new KalmanFilter(1, 1, 0.02f, 1, 0.02f, 0.0f);
KalmanFilter velZFilter = new KalmanFilter(1, 1, 0.02f, 1, 0.02f, 0.0f);
NoiseFilter velXSmooth = new NoiseFilter(6, 0.5f);
NoiseFilter velZSmooth = new NoiseFilter(6, 0.5f);
KalmanFilter yawRateFilter = new KalmanFilter(1, 1, 0.02f, 1, 0.02f, 0.0f);
NoiseFilter yawRateSmooth = new NoiseFilter(6, 0.5f);
NoiseFilter pitchFilter = new NoiseFilter(3);
NoiseFilter rollFilter = new NoiseFilter(3);
NoiseFilter yawFilter = new NoiseFilter(3);
KalmanFilter posXFilter = new KalmanFilter(1, 1, 0.02f, 1, 0.1f, 0.0f);
KalmanFilter posYFilter = new KalmanFilter(1, 1, 0.02f, 1, 0.1f, 0.0f);
KalmanFilter posZFilter = new KalmanFilter(1, 1, 0.02f, 1, 0.1f, 0.0f);
NestedSmooth posXSmooth = new NestedSmooth(12, 6, 0.5f);
NestedSmooth posYSmooth = new NestedSmooth(12, 6, 0.5f);
NestedSmooth posZSmooth = new NestedSmooth(12, 6, 0.5f);
NoiseFilter slipAngleSmooth = new NoiseFilter(6, 0.25f);
int readSize = 4 * 4 * 4;
byte[] readBuffer;
MemoryMappedFile mmf = null;
while (!isStopped)
{
try
{
float dt = (float)sw.ElapsedMilliseconds / 1000.0f;
while (true)
{
try
{
mmf = MemoryMappedFile.OpenExisting("Dirt5MatrixProvider");
if (mmf != null)
{
break;
}
else
{
Thread.Sleep(1000);
}
}
catch (FileNotFoundException)
{
Thread.Sleep(1000);
}
}
Mutex mutex = Mutex.OpenExisting("Dirt5MatrixProviderMutex");
mutex.WaitOne();
using (MemoryMappedViewStream stream = mmf.CreateViewStream())
{
BinaryReader reader = new BinaryReader(stream);
readBuffer = reader.ReadBytes(readSize);
}
mutex.ReleaseMutex();
float[] floats = new float[4 * 4];
Buffer.BlockCopy(readBuffer, 0, floats, 0, readBuffer.Length);
Matrix4x4 transform = new Matrix4x4(floats[0], floats[1], floats[2], floats[3]
, floats[4], floats[5], floats[6], floats[7]
, floats[8], floats[9], floats[10], floats[11]
, floats[12], floats[13], floats[14], floats[15]);
Vector3 rht = new Vector3(transform.M11, transform.M12, transform.M13);
Vector3 up = new Vector3(transform.M21, transform.M22, transform.M23);
Vector3 fwd = new Vector3(transform.M31, transform.M32, transform.M33);
float rhtMag = rht.Length();
float upMag = up.Length();
float fwdMag = fwd.Length();
//reading garbage
if (rhtMag < 0.9f || upMag < 0.9f || fwdMag < 0.9f)
{
IsConnected = false;
IsRunning = false;
break;
}
if (!lastFrameValid)
{
lastTransform = transform;
lastFrameValid = true;
lastVelocity = Vector3.Zero;
lastYaw = 0.0f;
continue;
}
DIRT5API telemetryData = new DIRT5API();
if (dt <= 0)
{
dt = 1.0f;
}
Vector3 worldVelocity = (transform.Translation - lastTransform.Translation) / dt;
lastTransform = transform;
Matrix4x4 rotation = new Matrix4x4();
rotation = transform;
rotation.M41 = 0.0f;
rotation.M42 = 0.0f;
rotation.M43 = 0.0f;
rotation.M44 = 1.0f;
Matrix4x4 rotInv = new Matrix4x4();
Matrix4x4.Invert(rotation, out rotInv);
Vector3 localVelocity = Vector3.Transform(worldVelocity, rotInv);
telemetryData.velX = worldVelocity.X;
telemetryData.velZ = worldVelocity.Z;
Vector3 localAcceleration = localVelocity - lastVelocity;
lastVelocity = localVelocity;
telemetryData.accX = localAcceleration.X * 10.0f;
telemetryData.accY = localAcceleration.Y * 100.0f;
telemetryData.accZ = localAcceleration.Z * 10.0f;
float pitch = (float)Math.Asin(-fwd.Y);
float yaw = (float)Math.Atan2(fwd.X, fwd.Z);
float roll = 0.0f;
Vector3 rhtPlane = rht;
rhtPlane.Y = 0;
rhtPlane = Vector3.Normalize(rhtPlane);
if (rhtPlane.Length() <= float.Epsilon)
{
roll = -(float)(Math.Sign(rht.Y) * Math.PI * 0.5f);
// Debug.WriteLine( "---Roll = " + roll + " " + Math.Sign( rht.Y ) );
}
else
{
roll = -(float)Math.Asin(Vector3.Dot(up, rhtPlane));
// Debug.WriteLine( "Roll = " + roll + " " + Math.Sign(rht.Y) );
}
// Debug.WriteLine( "" );
telemetryData.pitchPos = pitch;
telemetryData.yawPos = yaw;
telemetryData.rollPos = roll;
telemetryData.yawRate = CalculateAngularChange(lastYaw, yaw) * (180.0f / (float)Math.PI);
lastYaw = yaw;
// otherwise we are connected
IsConnected = true;
if (IsConnected)
{
IsRunning = true;
DIRT5API telemetryToSend = new DIRT5API();
telemetryToSend.Reset();
telemetryToSend.CopyFields(telemetryData);
telemetryToSend.accX = accXSmooth.Filter(telemetryData.accX);
telemetryToSend.accY = accYSmooth.Filter(telemetryData.accY);
telemetryToSend.accZ = accZSmooth.Filter(telemetryData.accZ);
telemetryToSend.pitchPos = pitchFilter.Filter(telemetryData.pitchPos);
telemetryToSend.rollPos = rollFilter.Filter(telemetryData.rollPos);
telemetryToSend.yawPos = yawFilter.Filter(telemetryData.yawPos);
telemetryToSend.velX = velXSmooth.Filter(velXFilter.Filter(telemetryData.velX));
telemetryToSend.velZ = velZSmooth.Filter(velZFilter.Filter(telemetryData.velZ));
telemetryToSend.yawRate = yawRateSmooth.Filter(yawRateFilter.Filter(telemetryData.yawRate));
telemetryToSend.yawAcc = slipAngleSmooth.Filter(telemetryToSend.CalculateSlipAngle());
sw.Restart();
TelemetryEventArgs args = new TelemetryEventArgs(
new DIRT5TelemetryInfo(telemetryToSend, lastTelemetryData));
RaiseEvent(OnTelemetryUpdate, args);
lastTelemetryData = telemetryToSend;
Thread.Sleep(1000 / 100);
}
else if (sw.ElapsedMilliseconds > 500)
{
IsRunning = false;
}
}
catch (Exception e)
{
LogError("DIRT5TelemetryProvider Exception while processing data", e);
mmf.Dispose();
IsConnected = false;
IsRunning = false;
Thread.Sleep(1000);
}
}
mmf.Dispose();
IsConnected = false;
IsRunning = false;
}
/// <summary>
/// The thread funktion to poll the telemetry data and send TelemetryUpdated events.
/// </summary>
private void Run()
{
BNGAPI lastTelemetryData = new BNGAPI();
lastTelemetryData.Reset();
Vector3 lastVelocity = Vector3.Zero;
Session session = new Session();
Stopwatch sw = new Stopwatch();
sw.Start();
KalmanFilter accXFilter = new KalmanFilter(1, 1, 0.02f, 1, 0.02f, 0.0f);
KalmanFilter accYFilter = new KalmanFilter(1, 1, 0.05f, 1, 0.1f, 0.0f);
KalmanFilter accZFilter = new KalmanFilter(1, 1, 0.05f, 1, 0.05f, 0.0f); //heave noisy af
NoiseFilter accXSmooth = new NoiseFilter(6, 1.0f);
NoiseFilter accYSmooth = new NoiseFilter(6, 0.5f);
NoiseFilter accZSmooth = new NoiseFilter(6, 1.0f);
KalmanFilter velXFilter = new KalmanFilter(1, 1, 0.02f, 1, 0.02f, 0.0f);
KalmanFilter velYFilter = new KalmanFilter(1, 1, 0.02f, 1, 0.02f, 0.0f);
NoiseFilter velXSmooth = new NoiseFilter(6, 0.5f);
NoiseFilter velYSmooth = new NoiseFilter(6, 0.5f);
KalmanFilter yawRateFilter = new KalmanFilter(1, 1, 0.02f, 1, 0.02f, 0.0f);
NoiseFilter yawRateSmooth = new NoiseFilter(6, 0.5f);
NoiseFilter pitchFilter = new NoiseFilter(3);
NoiseFilter rollFilter = new NoiseFilter(3);
NoiseFilter yawFilter = new NoiseFilter(3);
NoiseFilter slipAngleSmooth = new NoiseFilter(6, 0.25f);
UdpClient socket = new UdpClient();
socket.ExclusiveAddressUse = false;
socket.Client.Bind(new IPEndPoint(IPAddress.Any, PORTNUM));
Log("Listener started (port: " + PORTNUM.ToString() + ") BNGTelemetryProvider.Thread");
while (!isStopped)
{
try
{
// get data from game,
if (socket.Available == 0)
{
if (sw.ElapsedMilliseconds > 500)
{
IsRunning = false;
IsConnected = false;
Thread.Sleep(1000);
}
continue;
}
else
{
IsConnected = true;
}
Byte[] received = socket.Receive(ref _senderIP);
var alloc = GCHandle.Alloc(received, GCHandleType.Pinned);
BNGAPI telemetryData = (BNGAPI)Marshal.PtrToStructure(alloc.AddrOfPinnedObject(), typeof(BNGAPI));
// otherwise we are connected
IsConnected = true;
if (telemetryData.magic[0] == 'B' &&
telemetryData.magic[1] == 'N' &&
telemetryData.magic[2] == 'G' &&
telemetryData.magic[3] == '1')
{
IsRunning = true;
sw.Restart();
BNGAPI telemetryToSend = new BNGAPI();
telemetryToSend.Reset();
telemetryToSend.CopyFields(telemetryData);
telemetryToSend.accX = accXSmooth.Filter(accXFilter.Filter(telemetryData.accX));
telemetryToSend.accY = accYSmooth.Filter(accYFilter.Filter(telemetryData.accY));
telemetryToSend.accZ = accZSmooth.Filter(accZFilter.Filter(telemetryData.accZ));
telemetryToSend.pitchPos = pitchFilter.Filter(telemetryData.pitchPos);
telemetryToSend.rollPos = rollFilter.Filter(telemetryData.rollPos);
telemetryToSend.yawPos = yawFilter.Filter(telemetryData.yawPos);
telemetryToSend.velX = velXSmooth.Filter(velXFilter.Filter(telemetryData.velX));
telemetryToSend.velY = velYSmooth.Filter(velYFilter.Filter(telemetryData.velY));
telemetryToSend.yawRate = yawRateSmooth.Filter(yawRateFilter.Filter(telemetryData.yawRate));
telemetryToSend.yawAcc = slipAngleSmooth.Filter(telemetryToSend.CalculateSlipAngle());
TelemetryEventArgs args = new TelemetryEventArgs(
new BNGTelemetryInfo(telemetryToSend, lastTelemetryData));
RaiseEvent(OnTelemetryUpdate, args);
lastTelemetryData = telemetryData;
}
else if (sw.ElapsedMilliseconds > 500)
{
IsRunning = false;
}
}
catch (Exception e)
{
LogError("BNGTelemetryProvider Exception while processing data", e);
IsConnected = false;
IsRunning = false;
Thread.Sleep(1000);
}
}
socket.Close();
IsConnected = false;
IsRunning = false;
}
