public override List <string> GetDifferences(Chunk xiChunk)
{
FlatChunk lOther = xiChunk as FlatChunk;
if (DeclaredIdx != lOther.DeclaredIdx ||
DeclaredName != lOther.DeclaredName ||
!OriginPosition.Equals(lOther.OriginPosition) ||
!RotationVector.Equals(lOther.RotationVector) ||
Width != lOther.Width ||
Height != lOther.Height ||
ScaleX != lOther.ScaleX ||
ScaleY != lOther.ScaleY ||
HasMetaData != lOther.HasMetaData ||
FlgB != lOther.FlgB ||
FlgC != lOther.FlgC ||
Visible != lOther.Visible ||
FlgE != lOther.FlgE ||
!Utils.ArrayCompare(TerrainHeight, lOther.TerrainHeight) ||
!Utils.ArrayCompare(TextureIds, lOther.TextureIds) ||
!Utils.ArrayCompare(TexMetaData, lOther.TexMetaData))
{
List <string> lRet = base.GetDifferences(xiChunk);
lRet.Add("Changed flat #" + DeclaredIdx.ToString() + " (" + DeclaredName + ")");
return(lRet);
}
return(base.GetDifferences(xiChunk));
}
// Methods
/// <summary>
/// Updates the serialized properties from the non serialized properties.
/// </summary>
public void UpdateSerializedProperties()
{
RotationVectorValues = new double[3] {
RotationVector.Get(0), RotationVector.Get(1), RotationVector.Get(2)
};
TranslationVectorValues = new double[3] {
TranslationVector.Get(0), TranslationVector.Get(1), TranslationVector.Get(2)
};
}
// Methods
/// <summary>
/// Update the serialized properties from the non serialized properties.
/// </summary>
public void UpdateSerializedProperties()
{
RotationVectorValues = new double[3] { RotationVector.Get(0), RotationVector.Get(1), RotationVector.Get(2) };
TranslationVectorValues = new double[3] { TranslationVector.Get(0), TranslationVector.Get(1), TranslationVector.Get(2) };
RotationMatricesType = RotationMatrices[0].Type();
RotationMatricesValues = new double[CameraNumber][][];
CameraParameters.UpdatePropertyValues(RotationMatrices, RotationMatricesValues);
NewCameraMatricesType = NewCameraMatrices[0].Type();
NewCameraMatricesValues = new double[CameraNumber][][];
CameraParameters.UpdatePropertyValues(NewCameraMatrices, NewCameraMatricesValues);
}
/// <summary>
/// Processes WM_INPUT messages to retrieve information about any
/// keyboard events that occur.
/// </summary>
/// <param name="message">The WM_INPUT message to process.</param>
public void ProcessInputCommand(Message message)
{
uint dwSize = 0;
// First call to GetRawInputData sets the value of dwSize
// dwSize can then be used to allocate the appropriate amount of memory,
// storing the pointer in "buffer".
GetRawInputData(message.LParam,
RID_HEADER, IntPtr.Zero,
ref dwSize,
(uint)Marshal.SizeOf(typeof(RAWINPUTHEADER)));
IntPtr headerBuffer = Marshal.AllocHGlobal((int)dwSize);
try
{
// Check that buffer points to something, and if so,
// call GetRawInputData again to fill the allocated memory
// with information about the input
if (headerBuffer != IntPtr.Zero &&
GetRawInputData(message.LParam,
RID_HEADER,
headerBuffer,
ref dwSize,
(uint)Marshal.SizeOf(typeof(RAWINPUTHEADER))) == dwSize)
{
RAWINPUTHEADER header = (RAWINPUTHEADER)Marshal.PtrToStructure(headerBuffer, typeof(RAWINPUTHEADER));
if (header.dwType == RIM_TYPEHID)
{
DeviceInfo dInfo = null;
if (deviceList.Contains(header.hDevice))
{
dInfo = (DeviceInfo)deviceList[header.hDevice];
}
else
{
// Device not in list. Reenumerate all of them again. Could warn the code with some sort of Connect/Disconnect event.
EnumerateDevices();
dInfo = (DeviceInfo)deviceList[header.hDevice];
}
// The header tells us the size of the actual event
IntPtr eventBuffer = Marshal.AllocHGlobal(header.dwSize);
uint eventSize = (uint)header.dwSize;
if (eventBuffer != IntPtr.Zero &&
GetRawInputData(message.LParam,
RID_INPUT,
eventBuffer,
ref eventSize,
(uint)Marshal.SizeOf(typeof(RAWINPUTHEADER))) == header.dwSize)
{
RAW3DMOUSE_EVENTTYPE eventType = (RAW3DMOUSE_EVENTTYPE)Marshal.PtrToStructure(new IntPtr(eventBuffer.ToInt32() + Marshal.SizeOf(typeof(RAWINPUTHEADER))), typeof(RAW3DMOUSE_EVENTTYPE));
switch (eventType.eventType)
{
case (byte)RAW3DxMouseEventType.TranslationVector:
if (header.dwSize == Marshal.SizeOf(typeof(RAWINPUTHEADER)) + SIZEOF_STANDARD_REPORT) // standard length T report
{
RAW3DMOUSEMOTION_T t = (RAW3DMOUSEMOTION_T)Marshal.PtrToStructure(new IntPtr(eventBuffer.ToInt32() + Marshal.SizeOf(typeof(RAWINPUTHEADER))), typeof(RAW3DMOUSEMOTION_T));
TranslationVector tv = new TranslationVector(t.X_lb, t.X_hb, t.Y_lb, t.Y_hb, t.Z_lb, t.Z_hb);
// Console.Write("Motion Event = {0} {1} {2}", tv.X, tv.Y, tv.Z);
MotionEvent(this, new MotionEventArgs(dInfo, tv));
}
else // "High Speed" firmware version includes both T and R vector in the same report
{
RAW3DMOUSEMOTION_TR_COMBINED tr = (RAW3DMOUSEMOTION_TR_COMBINED)Marshal.PtrToStructure(new IntPtr(eventBuffer.ToInt32() + Marshal.SizeOf(typeof(RAWINPUTHEADER))), typeof(RAW3DMOUSEMOTION_TR_COMBINED));
TranslationVector tv = new TranslationVector(tr.X_lb, tr.X_hb, tr.Y_lb, tr.Y_hb, tr.Z_lb, tr.Z_hb);
RotationVector rv = new RotationVector(tr.RX_lb, tr.RX_hb, tr.RY_lb, tr.RY_hb, tr.RZ_lb, tr.RZ_hb);
// Console.WriteLine("6DOF Motion Event = {0} {1} {2} {3} {4} {5}", tv.X, tv.Y, tv.Z, rv.X, rv.Y, rv.Z);
MotionEvent(this, new MotionEventArgs(dInfo, tv, rv));
}
break;
case (byte)RAW3DxMouseEventType.RotationVector:
{
RAW3DMOUSEMOTION_R r = (RAW3DMOUSEMOTION_R)Marshal.PtrToStructure(new IntPtr(eventBuffer.ToInt32() + Marshal.SizeOf(typeof(RAWINPUTHEADER))), typeof(RAW3DMOUSEMOTION_R));
RotationVector rv = new RotationVector(r.X_lb, r.X_hb, r.Y_lb, r.Y_hb, r.Z_lb, r.Z_hb);
// Console.WriteLine(" {0} {1} {2}", rv.X, rv.Y, rv.Z);
MotionEvent(this, new MotionEventArgs(dInfo, rv));
}
break;
case (byte)RAW3DxMouseEventType.ButtonReport:
RAW3DMOUSEBUTTONS b = (RAW3DMOUSEBUTTONS)Marshal.PtrToStructure(new IntPtr(eventBuffer.ToInt32() + Marshal.SizeOf(typeof(RAWINPUTHEADER))), typeof(RAW3DMOUSEBUTTONS));
ButtonMask bm = new ButtonMask(b.b1, b.b2, b.b3, b.b4);
//Console.WriteLine("raw.buttons = {0:X}", bm.Pressed);
ButtonEvent(this, new ButtonEventArgs(dInfo, bm));
break;
}
}
}
}
}
finally
{
Marshal.FreeHGlobal(headerBuffer);
}
}
public MotionEventArgs(DeviceInfo dInfo, RotationVector rotationVector)
{
m_deviceInfo = dInfo;
m_rotationVector = rotationVector;
}
public MotionEventArgs(DeviceInfo dInfo, TranslationVector translationVector, RotationVector rotationVector)
{
m_deviceInfo = dInfo;
m_translationVector = translationVector;
m_rotationVector = rotationVector;
}
public override byte[] GetBytesForNextAction(RobotCursor cursor)
{
if (!cursor.ApplyNextAction(out _action))
{
return(null); // cursor buffer is empty
}
_params = null;
switch (_action.Type)
{
case ActionType.Translation:
case ActionType.Rotation:
case ActionType.Transformation:
RotationVector rv = cursor.rotation.AA.ToRotationVector();
_params = new int[]
{
_action.Id,
cursor.motionType == MotionType.Joint ? INST_MOVEJ_P : INST_MOVEL,
(int)Math.Round(cursor.position.X * 0.001 * FACTOR_M),
(int)Math.Round(cursor.position.Y * 0.001 * FACTOR_M),
(int)Math.Round(cursor.position.Z * 0.001 * FACTOR_M),
(int)Math.Round(rv.X * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(rv.Y * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(rv.Z * Geometry.TO_RADS * FACTOR_RAD)
};
// Workaround to SW3.0 crash problem: https://github.com/RobotExMachina/Machina.NET/issues/7
if (ZERO_PRECISION_ON_SAME_POSITION_MOTION && cursor.precision != 0 && cursor.prevPosition != null && cursor.position.IsSimilar(cursor.prevPosition))
{
Logger.Debug("Applying ZERO_PRECISION_ON_SAME_POSITION_MOTION");
_params = WrapParamsWithZeroPrecision(_params, cursor);
}
break;
case ActionType.Axes:
_params = new int[]
{
_action.Id,
INST_MOVEJ_Q,
(int)Math.Round(cursor.axes.J1 * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(cursor.axes.J2 * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(cursor.axes.J3 * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(cursor.axes.J4 * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(cursor.axes.J5 * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(cursor.axes.J6 * Geometry.TO_RADS * FACTOR_RAD),
};
// Another ZERO_PRECISION_ON_SAME_POSITION_MOTION check should happen here for 6th axis rotation only...
break;
case ActionType.Wait:
ActionWait aa = _action as ActionWait;
_params = new int[]
{
_action.Id,
INST_SLEEP,
(int)Math.Round(aa.millis * 0.001 * FACTOR_SEC)
};
break;
// Not implemented yet
//case ActionType.Message:
// ActionMessage am = (ActionMessage)action;
// dec = string.Format(" popup(\"{0}\", title=\"Machina Message\", warning=False, error=False)",
// am.message);
// break;
case ActionType.AttachTool:
//ActionAttachTool aatt = _action as ActionAttachTool;
//Tool t = aatt.tool;
Tool t = cursor.tool; // can cursor.tool be null? The action would have not gone through if there wasn't a tool available for attachment, right?
RotationVector trv = t.TCPOrientation.ToRotationVector();
_params = new int[]
{
_action.Id,
INST_SET_TOOL,
(int)Math.Round(t.TCPPosition.X * 0.001 * FACTOR_M),
(int)Math.Round(t.TCPPosition.Y * 0.001 * FACTOR_M),
(int)Math.Round(t.TCPPosition.Z * 0.001 * FACTOR_M),
(int)Math.Round(trv.X * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(trv.Y * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(trv.Z * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(t.Weight * FACTOR_KG)
};
break;
case ActionType.DetachTool:
_params = new int[]
{
_action.Id,
INST_SET_TOOL,
0,
0,
0,
0,
0,
0,
0
};
break;
case ActionType.IODigital:
ActionIODigital aiod = _action as ActionIODigital;
_params = new int[]
{
_action.Id,
INST_SET_DIGITAL_OUT,
aiod.pinNum,
aiod.on ? 1 : 0,
aiod.isToolPin ? 1 : 0
};
break;
case ActionType.IOAnalog:
ActionIOAnalog aioa = _action as ActionIOAnalog;
_params = new int[]
{
_action.Id,
INST_SET_ANALOG_OUT,
aioa.pinNum,
(int)Math.Round(aioa.value * FACTOR_VOLT)
//aioa.isToolPin ? 1 : 0 // there is no analog out on the tool
};
break;
// ╔═╗╔═╗╔╦╗╔╦╗╦╔╗╔╔═╗╔═╗
// ╚═╗║╣ ║ ║ ║║║║║ ╦╚═╗
// ╚═╝╚═╝ ╩ ╩ ╩╝╚╝╚═╝╚═╝
// Speed is now set globally, and the driver takes care of translating that internally
case ActionType.Speed:
_params = new int[]
{
_action.Id,
INST_ALL_SPEED,
(int)Math.Round(cursor.speed * 0.001 * FACTOR_M)
};
break;
// Idem for acceleration
case ActionType.Acceleration:
_params = new int[]
{
_action.Id,
INST_ALL_ACC,
(int)Math.Round(cursor.acceleration * 0.001 * FACTOR_M)
};
break;
case ActionType.Precision:
_params = new int[]
{
_action.Id,
INST_BLEND,
(int)Math.Round(cursor.precision * 0.001 * FACTOR_M)
};
break;
case ActionType.PushPop:
ActionPushPop app = _action as ActionPushPop;
if (app.push)
{
break;
}
Settings beforePop = cursor.settingsBuffer.SettingsBeforeLastPop;
Dictionary <int, int> poppedSettings = new Dictionary <int, int>();
// These are the states kept in the controller as of v1.0 of the driver
//if (beforePop.Speed != cursor.speed)
// poppedSettings.Add(INST_TCP_SPEED, (int)Math.Round(cursor.speed * 0.001 * FACTOR_M));
//if (beforePop.Acceleration != cursor.acceleration)
// poppedSettings.Add(INST_TCP_ACC, (int)Math.Round(cursor.acceleration * 0.001 * FACTOR_M));
//if (beforePop.JointSpeed != cursor.jointSpeed)
// poppedSettings.Add(INST_Q_SPEED, (int)Math.Round(cursor.jointSpeed * Geometry.TO_RADS * FACTOR_RAD));
//if (beforePop.JointAcceleration != cursor.jointAcceleration)
// poppedSettings.Add(INST_Q_ACC, (int)Math.Round(cursor.jointAcceleration * Geometry.TO_RADS * FACTOR_RAD));
// These are the states kept in the controller as of v1.0 of the driver
if (beforePop.Speed != cursor.speed)
{
poppedSettings.Add(INST_ALL_SPEED, (int)Math.Round(cursor.speed * 0.001 * FACTOR_M));
}
if (beforePop.Acceleration != cursor.acceleration)
{
poppedSettings.Add(INST_ALL_ACC, (int)Math.Round(cursor.acceleration * 0.001 * FACTOR_M));
}
if (beforePop.Precision != cursor.precision)
{
poppedSettings.Add(INST_BLEND, (int)Math.Round(cursor.precision * 0.001 * FACTOR_M));
}
// Generate a buffer with all instructions, ids of -1 except for the last one.
_params = new int[3 * poppedSettings.Count];
int it = 0;
foreach (var setting in poppedSettings)
{
_params[3 * it] = it == poppedSettings.Count - 1 ? app.Id : -1; // only attach the real id to the last instruction
_params[3 * it + 1] = setting.Key;
_params[3 * it + 2] = setting.Value;
it++;
}
break;
case ActionType.Coordinates:
throw new NotImplementedException(); // @TODO: this should also change the WObj, but not on it yet...
//// Send comma-separated integers
//case ActionType.CustomCode:
// ActionCustomCode acc = _action as ActionCustomCode;
// int[] values;
// if (Numeric.CommaSeparatedStringToInts(out values))
// {
// }
// else
// {
// Logger.Warning("Invalid CustomCode: please use a string of comma-separated integers, like \"1,);
// }
// break;
// If the Action wasn't on the list above, it doesn't have a message representation...
default:
Logger.Verbose("Cannot stream action `" + _action + "`");
return(null);
}
if (_params == null)
{
return(null);
}
_buffer = Utilities.Conversion.Int32ArrayToByteArray(_params, false);
return(_buffer);
}
public void RotationVector_Creation_NegativeAngle()
{
RotationVector rv;
double x, y, z, angle;
double len, len2;
Vector v1, v2;
// Test random axes
for (var i = 0; i < 50; i++)
{
x = Random(-100, 100);
y = Random(-100, 100);
z = Random(-100, 100);
angle = Random(-720, 0);
Trace.WriteLine("");
Trace.WriteLine(x + " " + y + " " + z + " " + angle);
rv = new RotationVector(x, y, z, angle);
Trace.WriteLine(rv);
v1 = new Vector(x, y, z);
v1.Normalize();
v1.Invert();
v2 = rv.GetVector();
Assert.IsTrue(v1.IsSimilar(v2));
// Raw check
len = Math.Sqrt(x * x + y * y + z * z);
len2 = Math.Sqrt(rv.X * rv.X + rv.Y * rv.Y + rv.Z * rv.Z);
Trace.WriteLine(len);
Trace.WriteLine(len2);
Assert.AreNotEqual(len, len2, 0.000001);
Assert.AreEqual(-angle, len2, 0.000001);
Assert.AreEqual(-angle, rv.GetAngle(), 0.000001);
}
// Test all permutations of unitary components (including zero)
for (x = -1; x <= 1; x++)
{
for (y = -1; y <= 1; y++)
{
for (z = -1; z <= 1; z++)
{
for (angle = 720; angle <= 0; angle += 45)
{
Trace.WriteLine("");
Trace.WriteLine(x + " " + y + " " + z + " " + angle);
rv = new RotationVector(x, y, z, angle);
Trace.WriteLine(rv);
// Raw check
len = Math.Sqrt(x * x + y * y + z * z);
len2 = Math.Sqrt(rv.X * rv.X + rv.Y * rv.Y + rv.Z * rv.Z);
Trace.WriteLine(len);
Trace.WriteLine(len2);
if (angle == 0 || len == 0)
{
Assert.IsTrue(rv.IsZero());
Assert.AreEqual(0, rv.GetAngle(), 0.000001);
}
else
{
Assert.AreNotEqual(len, len2, 0.000001);
Assert.AreEqual(-angle, len2, 0.000001);
Assert.AreEqual(-angle, rv.GetAngle(), 0.000001);
v1 = new Vector(x, y, z);
v1.Normalize();
v2 = rv.GetVector();
v2.Invert();
Assert.IsTrue(v1.IsSimilar(v2));
}
}
}
}
}
}
public void RotationVector_ToQuaternion_ToRotationVector()
{
RotationVector rv1, rv2;
Quaternion q;
double x, y, z, angle;
double len;
// Test random axes
for (var i = 0; i < 50; i++)
{
x = Random(-100, 100);
y = Random(-100, 100);
z = Random(-100, 100);
angle = Random(-1440, 1440);
Trace.WriteLine("");
Trace.WriteLine(x + " " + y + " " + z + " " + angle);
rv1 = new RotationVector(x, y, z, angle);
q = rv1.ToQuaternion();
rv2 = q.ToRotationVector(false);
Trace.WriteLine(rv1 + " (" + rv1.ToAxisAngle() + ")");
Trace.WriteLine(q + " (" + q.ToAxisAngle() + ")");
Trace.WriteLine(rv2 + " (" + rv2.ToAxisAngle() + ")");
//rv1 = new RotationVector(x, y, z, angle);
//aa1 = rv1.ToAxisAngle();
//q = aa1.ToQuaternion();
//rv2 = q.ToRotationVector();
//Trace.WriteLine("Itemized:");
//Trace.WriteLine(rv1 + " (" + rv1.ToAxisAngle() + ")");
//Trace.WriteLine(aa1);
//Trace.WriteLine(q + " (" + q.ToAxisAngle() + ")");
//Trace.WriteLine(rv2 + " (" + rv2.ToAxisAngle() + ")");
////Assert.IsTrue(rv1 == rv2);
////Assert.AreEqual(angle > 0 ? angle : -angle, rv2.GetAngle(), 0.00001);
// This is not very clean, but I guess does the job...?
//Assert.IsTrue(rv1.ToAxisAngle().IsEquivalent(rv2.ToAxisAngle()));
Assert.IsTrue(rv1.IsEquivalent(rv2));
}
// Test all permutations of unitary components (including zero)
for (x = -1; x <= 1; x++)
{
for (y = -1; y <= 1; y++)
{
for (z = -1; z <= 1; z++)
{
for (angle = -720; angle <= 0; angle += 45)
{
Trace.WriteLine("");
Trace.WriteLine(x + " " + y + " " + z + " " + angle);
rv1 = new RotationVector(x, y, z, angle);
q = rv1.ToQuaternion();
rv2 = q.ToRotationVector(false);
Trace.WriteLine(rv1 + " (" + rv1.ToAxisAngle() + ")");
Trace.WriteLine(q + " (" + q.ToAxisAngle() + ")");
Trace.WriteLine(rv2 + " (" + rv2.ToAxisAngle() + ")");
len = rv1.Length();
if (angle == 0 || len == 0)
{
Assert.IsTrue(q.IsIdentity());
Assert.IsTrue(rv2.IsZero());
}
else
{
//Assert.IsTrue(rv1.ToAxisAngle().IsEquivalent(rv2.ToAxisAngle()), "RV assert failed");
if (x == -1 && y == -1 && z == -1 && angle == -360)
{
Assert.IsTrue(rv1.IsEquivalent(rv2));
}
Assert.IsTrue(rv1.IsEquivalent(rv2));
}
}
}
}
}
}
public void RotationVector_ToAxisAngle_ToRotationVector()
{
RotationVector rv1, rv2;
AxisAngle aa;
double x, y, z, angle;
double len;
// Test random axes
for (var i = 0; i < 50; i++)
{
x = Random(-100, 100);
y = Random(-100, 100);
z = Random(-100, 100);
angle = Random(-720, 720);
Trace.WriteLine("");
Trace.WriteLine(x + " " + y + " " + z + " " + angle);
rv1 = new RotationVector(x, y, z, angle);
aa = rv1.ToAxisAngle();
rv2 = aa.ToRotationVector();
Trace.WriteLine(rv1);
Trace.WriteLine(aa);
Trace.WriteLine(rv2);
Assert.IsTrue(rv1.IsSimilar(rv2));
Assert.AreEqual(angle > 0 ? angle : -angle, aa.Angle, 0.00001);
}
// Test all permutations of unitary components (including zero)
for (x = -1; x <= 1; x++)
{
for (y = -1; y <= 1; y++)
{
for (z = -1; z <= 1; z++)
{
for (angle = -720; angle <= 0; angle += 45)
{
Trace.WriteLine("");
Trace.WriteLine(x + " " + y + " " + z + " " + angle);
rv1 = new RotationVector(x, y, z, angle);
aa = rv1.ToAxisAngle();
rv2 = aa.ToRotationVector();
Trace.WriteLine(rv1);
Trace.WriteLine(aa);
Trace.WriteLine(rv2);
len = rv1.Length();
if (angle == 0 || len == 0)
{
Assert.IsTrue(aa.IsZero());
Assert.IsTrue(rv2.IsZero());
}
else
{
Assert.IsTrue(rv1.IsSimilar(rv2));
Assert.AreEqual(angle > 0 ? angle : -angle, aa.Angle, 0.00001);
}
}
}
}
}
}
public override byte[] GetBytesForNextAction(RobotCursor cursor)
{
if (!cursor.ApplyNextAction(out _action))
{
return(null); // cursor buffer is empty
}
_params = null;
switch (_action.type)
{
case ActionType.Translation:
case ActionType.Rotation:
case ActionType.Transformation:
RotationVector rv = cursor.rotation.AA.ToRotationVector();
_params = new int[]
{
_action.id,
cursor.motionType == MotionType.Joint ? INST_MOVEJ_P : INST_MOVEL,
(int)Math.Round(cursor.position.X * 0.001 * FACTOR_M),
(int)Math.Round(cursor.position.Y * 0.001 * FACTOR_M),
(int)Math.Round(cursor.position.Z * 0.001 * FACTOR_M),
(int)Math.Round(rv.X * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(rv.Y * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(rv.Z * Geometry.TO_RADS * FACTOR_RAD)
};
break;
case ActionType.Axes:
_params = new int[]
{
_action.id,
INST_MOVEJ_Q,
(int)Math.Round(cursor.joints.J1 * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(cursor.joints.J2 * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(cursor.joints.J3 * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(cursor.joints.J4 * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(cursor.joints.J5 * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(cursor.joints.J6 * Geometry.TO_RADS * FACTOR_RAD),
};
break;
case ActionType.Wait:
ActionWait aa = _action as ActionWait;
_params = new int[]
{
_action.id,
INST_SLEEP,
(int)Math.Round(aa.millis * 0.001 * FACTOR_SEC)
};
break;
// Not implemented yet
//case ActionType.Message:
// ActionMessage am = (ActionMessage)action;
// dec = string.Format(" popup(\"{0}\", title=\"Machina Message\", warning=False, error=False)",
// am.message);
// break;
case ActionType.Attach:
ActionAttach aatt = _action as ActionAttach;
Tool t = aatt.tool;
RotationVector trv = t.TCPOrientation.ToRotationVector();
_params = new int[]
{
_action.id,
INST_SET_TOOL,
(int)Math.Round(t.TCPPosition.X * 0.001 * FACTOR_M),
(int)Math.Round(t.TCPPosition.Y * 0.001 * FACTOR_M),
(int)Math.Round(t.TCPPosition.Z * 0.001 * FACTOR_M),
(int)Math.Round(trv.X * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(trv.Y * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(trv.Z * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(t.Weight * FACTOR_M)
};
break;
case ActionType.Detach:
_params = new int[]
{
_action.id,
INST_SET_TOOL,
0,
0,
0,
0,
0,
0,
0
};
break;
case ActionType.IODigital:
ActionIODigital aiod = _action as ActionIODigital;
_params = new int[]
{
_action.id,
INST_SET_DIGITAL_OUT,
aiod.pinNum,
aiod.on ? 1 : 0
};
break;
case ActionType.IOAnalog:
ActionIOAnalog aioa = _action as ActionIOAnalog;
_params = new int[]
{
_action.id,
INST_SET_DIGITAL_OUT,
aioa.pinNum,
(int)Math.Round(aioa.value * FACTOR_VOLT)
};
break;
// ╔═╗╔═╗╔╦╗╔╦╗╦╔╗╔╔═╗╔═╗
// ╚═╗║╣ ║ ║ ║║║║║ ╦╚═╗
// ╚═╝╚═╝ ╩ ╩ ╩╝╚╝╚═╝╚═╝
case ActionType.Speed:
_params = new int[]
{
_action.id,
INST_TCP_SPEED,
(int)Math.Round(cursor.speed * 0.001 * FACTOR_M)
};
break;
case ActionType.Acceleration:
_params = new int[]
{
_action.id,
INST_TCP_ACC,
(int)Math.Round(cursor.acceleration * 0.001 * FACTOR_M)
};
break;
case ActionType.JointSpeed:
_params = new int[]
{
_action.id,
INST_Q_SPEED,
(int)Math.Round(cursor.jointSpeed * Geometry.TO_RADS * FACTOR_RAD)
};
break;
case ActionType.JointAcceleration:
_params = new int[]
{
_action.id,
INST_Q_ACC,
(int)Math.Round(cursor.jointAcceleration * Geometry.TO_RADS * FACTOR_RAD)
};
break;
case ActionType.Precision:
_params = new int[]
{
_action.id,
INST_BLEND,
(int)Math.Round(cursor.precision * 0.001 * FACTOR_M)
};
break;
case ActionType.PushPop:
ActionPushPop app = _action as ActionPushPop;
if (app.push)
{
break;
}
Settings beforePop = cursor.settingsBuffer.SettingsBeforeLastPop;
Dictionary <int, int> poppedSettings = new Dictionary <int, int>();
// These are the states kept in the controller as of v1.0 of the driver
if (beforePop.Speed != cursor.speed)
{
poppedSettings.Add(INST_TCP_SPEED, (int)Math.Round(cursor.speed * 0.001 * FACTOR_M));
}
if (beforePop.Acceleration != cursor.acceleration)
{
poppedSettings.Add(INST_TCP_ACC, (int)Math.Round(cursor.acceleration * 0.001 * FACTOR_M));
}
if (beforePop.JointSpeed != cursor.jointSpeed)
{
poppedSettings.Add(INST_Q_SPEED, (int)Math.Round(cursor.jointSpeed * Geometry.TO_RADS * FACTOR_RAD));
}
if (beforePop.JointAcceleration != cursor.jointAcceleration)
{
poppedSettings.Add(INST_Q_ACC, (int)Math.Round(cursor.jointAcceleration * Geometry.TO_RADS * FACTOR_RAD));
}
if (beforePop.Precision != cursor.precision)
{
poppedSettings.Add(INST_BLEND, (int)Math.Round(cursor.precision * 0.001 * FACTOR_M));
}
// Generate a buffer with all instructions, ids of -1 except for the last one.
_params = new int[3 * poppedSettings.Count];
int it = 0;
foreach (var setting in poppedSettings)
{
_params[3 * it] = it == poppedSettings.Count - 1 ? app.id : -1; // only attach the real id to the last instruction
_params[3 * it + 1] = setting.Key;
_params[3 * it + 2] = setting.Value;
it++;
}
break;
case ActionType.Coordinates:
throw new NotImplementedException(); // @TODO: this should also change the WObj, but not on it yet...
}
if (_params == null)
{
return(null);
}
_buffer = Util.Int32ArrayToByteArray(_params, false);
return(_buffer);
}
public MotionEventArgs(DeviceInfo dInfo, RotationVector rotationVector)
{
m_deviceInfo = dInfo;
m_rotationVector = rotationVector;
}
public MotionEventArgs(DeviceInfo dInfo, TranslationVector translationVector, RotationVector rotationVector)
{
m_deviceInfo = dInfo;
m_translationVector = translationVector;
m_rotationVector = rotationVector;
}
/// <summary>
/// Processes WM_INPUT messages to retrieve information about any
/// keyboard events that occur.
/// </summary>
/// <param name="message">The WM_INPUT message to process.</param>
public void ProcessInputCommand(Message message)
{
uint dwSize = 0;
// First call to GetRawInputData sets the value of dwSize
// dwSize can then be used to allocate the appropriate amount of memory,
// storing the pointer in "buffer".
GetRawInputData(message.LParam,
RID_HEADER, IntPtr.Zero,
ref dwSize,
(uint)Marshal.SizeOf(typeof(RAWINPUTHEADER)));
var headerBuffer = Marshal.AllocHGlobal((int)dwSize);
try
{
// Check that buffer points to something, and if so,
// call GetRawInputData again to fill the allocated memory
// with information about the input
if (headerBuffer != IntPtr.Zero &&
GetRawInputData(message.LParam,
RID_HEADER,
headerBuffer,
ref dwSize,
(uint)Marshal.SizeOf(typeof(RAWINPUTHEADER))) == dwSize)
{
var header = (RAWINPUTHEADER)Marshal.PtrToStructure(headerBuffer, typeof(RAWINPUTHEADER));
if (header.dwType == RIM_TYPEHID)
{
DeviceInfo dInfo = null;
if (deviceList.Contains(header.hDevice))
{
dInfo = (DeviceInfo)deviceList[header.hDevice];
}
else
{
// Device not in list. Reenumerate all of them again. Could warn the code with some sort of Connect/Disconnect event.
EnumerateDevices();
dInfo = (DeviceInfo)deviceList[header.hDevice];
}
// The header tells us the size of the actual event
var eventBuffer = Marshal.AllocHGlobal(header.dwSize);
var eventSize = (uint)header.dwSize;
if (eventBuffer != IntPtr.Zero &&
GetRawInputData(message.LParam,
RID_INPUT,
eventBuffer,
ref eventSize,
(uint)Marshal.SizeOf(typeof(RAWINPUTHEADER))) == header.dwSize)
{
var eventType = (RAW3DMOUSE_EVENTTYPE)Marshal.PtrToStructure(new IntPtr(eventBuffer.ToInt32() + Marshal.SizeOf(typeof(RAWINPUTHEADER))), typeof(RAW3DMOUSE_EVENTTYPE));
switch (eventType.eventType)
{
case (byte)RAW3DxMouseEventType.TranslationVector:
if (header.dwSize == Marshal.SizeOf(typeof(RAWINPUTHEADER)) + SIZEOF_STANDARD_REPORT) // standard length T report
{
var t = (RAW3DMOUSEMOTION_T)Marshal.PtrToStructure(new IntPtr(eventBuffer.ToInt32() + Marshal.SizeOf(typeof(RAWINPUTHEADER))), typeof(RAW3DMOUSEMOTION_T));
var tv = new TranslationVector(t.X_lb, t.X_hb, t.Y_lb, t.Y_hb, t.Z_lb, t.Z_hb);
// Console.Write("Motion Event = {0} {1} {2}", tv.X, tv.Y, tv.Z);
MotionEvent(this, new MotionEventArgs(dInfo, tv));
}
else // "High Speed" firmware version includes both T and R vector in the same report
{
var tr = (RAW3DMOUSEMOTION_TR_COMBINED)Marshal.PtrToStructure(new IntPtr(eventBuffer.ToInt32() + Marshal.SizeOf(typeof(RAWINPUTHEADER))), typeof(RAW3DMOUSEMOTION_TR_COMBINED));
var tv = new TranslationVector(tr.X_lb, tr.X_hb, tr.Y_lb, tr.Y_hb, tr.Z_lb, tr.Z_hb);
var rv = new RotationVector(tr.RX_lb, tr.RX_hb, tr.RY_lb, tr.RY_hb, tr.RZ_lb, tr.RZ_hb);
// Console.WriteLine("6DOF Motion Event = {0} {1} {2} {3} {4} {5}", tv.X, tv.Y, tv.Z, rv.X, rv.Y, rv.Z);
MotionEvent(this, new MotionEventArgs(dInfo, tv, rv));
}
break;
case (byte)RAW3DxMouseEventType.RotationVector:
{
var r = (RAW3DMOUSEMOTION_R)Marshal.PtrToStructure(new IntPtr(eventBuffer.ToInt32() + Marshal.SizeOf(typeof(RAWINPUTHEADER))), typeof(RAW3DMOUSEMOTION_R));
var rv = new RotationVector(r.X_lb, r.X_hb, r.Y_lb, r.Y_hb, r.Z_lb, r.Z_hb);
// Console.WriteLine(" {0} {1} {2}", rv.X, rv.Y, rv.Z);
MotionEvent(this, new MotionEventArgs(dInfo, rv));
}
break;
case (byte)RAW3DxMouseEventType.ButtonReport:
var b = (RAW3DMOUSEBUTTONS)Marshal.PtrToStructure(new IntPtr(eventBuffer.ToInt32() + Marshal.SizeOf(typeof(RAWINPUTHEADER))), typeof(RAW3DMOUSEBUTTONS));
var bm = new ButtonMask(b.b1, b.b2, b.b3, b.b4);
Console.WriteLine("raw.buttons = {0:X}", bm.Pressed);
ButtonEvent(this, new ButtonEventArgs(dInfo, bm));
break;
}
}
}
}
}
finally
{
Marshal.FreeHGlobal(headerBuffer);
}
}
private GLTK.Entity GetSurfaceEntity(Level xiLevel, eTextureMode xiTextureMode, eTexMetaDataEntries xiSelectedMetadata)
{
// notes:
// invert the textures along the y-axis
// use level co-ords, so z is down
/////////////////////////////////////////////////////
// The surface
Entity lSurface = new MMEdEntity(this);
Font lNumberFont = null;
Brush lNumberFGBrush = null, lNumberBGBrush = null;
Pen lWaypointPen = null, lKeyWaypointPen = null;
if (xiTextureMode == eTextureMode.NormalTexturesWithMetadata)
{
lNumberFont = new Font(FontFamily.GenericMonospace, 10);
lNumberFGBrush = new SolidBrush(Color.Black);
lNumberBGBrush = new SolidBrush(Color.White);
lWaypointPen = new Pen(Color.Black, 1f);
lKeyWaypointPen = new Pen(Color.Red, 2f);
}
for (int x = 0; x < Width; x++)
{
for (int y = 0; y < Height; y++)
{
Mesh lSquare = new OwnedMesh(this, PolygonMode.Quads);
lSquare.AddFace(
new Vertex(new Point(x, y, -GetTerrainHeightSafe(x, y)), 0, 0),
new Vertex(new Point(x + 1, y, -GetTerrainHeightSafe(x + 1, y)), 1, 0),
new Vertex(new Point(x + 1, y + 1, -GetTerrainHeightSafe(x + 1, y + 1)), 1, 1),
new Vertex(new Point(x, y + 1, -GetTerrainHeightSafe(x, y + 1)), 0, 1));
switch (xiTextureMode)
{
case eTextureMode.WireFrame:
lSquare.RenderMode = RenderMode.Wireframe;
break;
// normal textures, optionally with metadata drawn on
case eTextureMode.NormalTextures:
case eTextureMode.NormalTexturesWithMetadata:
TIMChunk lTIM = xiLevel.GetTileById(TextureIds[x][y]);
if (lTIM != null)
{
//some TIMs can't be loaded yet: they're null
Bitmap lTexture = lTIM.ToBitmap();
if (xiTextureMode == eTextureMode.NormalTexturesWithMetadata &&
TexMetaData != null)
{
byte lVal = TexMetaData[x][y][(int)xiSelectedMetadata];
if (lVal != 0)
{
// we create a new bitmap based on the given texture
// a) so that we can modify it freely
// and b) to change it from indexed to full colour mode, to allow us
// to draw on it (otherwise we'll get an exception)
lTexture = new Bitmap(lTexture);
Graphics g = Graphics.FromImage(lTexture);
string lText = lVal.ToString();
SizeF size = g.MeasureString(lText, lNumberFont);
float xf = lTexture.Width / 2.0f - size.Width / 2.0f;
float yf = lTexture.Height / 2.0f - size.Height / 2.0f;
g.FillRectangle(lNumberBGBrush, xf, yf, size.Width, size.Height);
g.DrawString(
lText,
lNumberFont,
lNumberFGBrush,
xf,
yf);
if (xiSelectedMetadata == eTexMetaDataEntries.Waypoint)
{
Pen lPen = xiLevel.WaypointIsKeyWaypoint(lVal)
? lKeyWaypointPen
: lWaypointPen;
g.DrawRectangle(
lPen,
0, 0, lTexture.Width - 1, lTexture.Height - 1);
}
}
}
lSquare.Texture = AbstractRenderer.ImageToTextureId(lTexture);
}
break;
//draw the bumpmap textures on:
case eTextureMode.BumpmapTextures:
if (TexMetaData != null)
{
BumpImageChunk lBIC = xiLevel.GetBumpById(TexMetaData[x][y][(int)eTexMetaDataEntries.Bumpmap]);
if (lBIC != null)
{
Bitmap lTexture = lBIC.ToBitmap();
lSquare.Texture = AbstractRenderer.ImageToTextureId(lTexture);
}
}
break;
default: throw new Exception("Unexpected case");
} //end switch
lSurface.Meshes.Add(lSquare);
}
}
lSurface.Scale(ScaleX, ScaleY, 1.0);
if (RotationVector.Norm() != 0)
{
//the rotation is z-y-x
lSurface.RotateAboutWorldOrigin(RotationVector.Z / 1024.0 * Math.PI / 2.0, Vector.ZAxis);
lSurface.RotateAboutWorldOrigin(-RotationVector.Y / 1024.0 * Math.PI / 2.0, Vector.YAxis);
lSurface.RotateAboutWorldOrigin(-RotationVector.X / 1024.0 * Math.PI / 2.0, Vector.XAxis);
}
Point lNewPos = ThreeDeeViewer.Short3CoordToPoint(OriginPosition);
lSurface.Position = new Point(lNewPos.x, lNewPos.y, -lNewPos.z);
return(lSurface);
}
