public static SPlatform CreateEmpty(uint numberPointsOnGround, uint numberOfInstruments, uint numberOfAxes)
{
var pointsArray = new SPoint3D[numberPointsOnGround].Set(SPoint3D.Default());
var axisArray = new SAxis[numberOfAxes].Set(SAxis.Default());
var axisArray_m = MarshalArray(axisArray);
var childPointers = s_childPointerLut[axisArray_m];
var instrumentsArray = new SInstrument[numberOfInstruments].Set(SInstrument.Default(childPointers));
return(new SPlatform()
{
m_pcPlatformId = NotSet.ToPtr(),
m_oPlatform2Ground = STransformation.Default(),
m_oBoundingBox = SBoundingBox.Default(),
m_nNrOfPlatformPointsOnGround = numberPointsOnGround,
m_poPointsOnGround = MarshalArray(pointsArray),
m_nNrOfPlatformInstruments = numberOfInstruments,
m_poPlatformInstruments = MarshalArray(instrumentsArray),
m_nNrOfPlatformAxes = numberOfAxes,
m_poPlatformAxes = axisArray_m
});
}
public double getAxis(Joy joy, SAxis axis)
{
if (axis.axis == 0 || Math.Abs(axis.axis) > joy.axes.Length)
{
ROS.Error("Axis " + axis.axis + " out of range, joy has " + joy.axes.Length + " axes");
return(0);
}
double output = Math.Abs(axis.axis) / axis.axis * joy.axes[Math.Abs(axis.axis) - 1] * axis.factor + axis.offset;
// TODO keep or remove deadzone? may not be needed
// if (Math.Abs(output) < axis.max_ * 0.2)
// {
// output = 0.0;
// }
return(output);
}
