void _HandleTwists()
{
Twist resultGesture = Twist.None;
float resultAngle = 0;
if (Input.touchCount == 0 && isGestureGoing)
{
isGestureGoing = false;
resultGesture = Twist.Ending;
}
if (Input.touchCount == 2)
{
var touch0 = Input.GetTouch(0);
var touch1 = Input.GetTouch(1);
var touch0Direction = touch0.position - touch0.deltaPosition;
var touch1Direction = touch1.position - touch1.deltaPosition;
if (touch1Direction != touch1.position)
{
resultAngle = Vector3.SignedAngle(touch1.position - touch0.position, touch1Direction - touch0Direction, Camera.main.transform.forward);
resultAngle = (Camera.main.transform.forward.z < 0 ? resultAngle * -1 : resultAngle);
resultGesture = Mathf.Abs(resultAngle) > minTwistAngle ? Twist.Twist : Twist.None;
}
}
if (resultGesture != Twist.None && _IsActionPermited(resultGesture))
{
isGestureGoing = (resultGesture != Twist.Ending);
_NotifyAboutTwist(resultGesture, resultAngle);
}
}
public Twist limit (Twist input)
{
Twist output = new Twist ();
output.linear = linear.limit ( input.linear );
output.angular = angular.limit ( input.angular );
return output;
}
public FrameVel(Frame _T, Twist _t) : this(kdlPINVOKE.new_FrameVel__SWIG_2(Frame.getCPtr(_T), Twist.getCPtr(_t)), true)
{
if (kdlPINVOKE.SWIGPendingException.Pending)
{
throw kdlPINVOKE.SWIGPendingException.Retrieve();
}
}
public override void Deserialize(byte[] SERIALIZEDSTUFF, ref int currentIndex)
{
int arraylength = -1;
bool hasmetacomponents = false;
object __thing;
int piecesize = 0;
byte[] thischunk, scratch1, scratch2;
IntPtr h;
//link_name
link_name = "";
piecesize = BitConverter.ToInt32(SERIALIZEDSTUFF, currentIndex);
currentIndex += 4;
link_name = Encoding.ASCII.GetString(SERIALIZEDSTUFF, currentIndex, piecesize);
currentIndex += piecesize;
//pose
pose = new Pose(SERIALIZEDSTUFF, ref currentIndex);
//twist
twist = new Twist(SERIALIZEDSTUFF, ref currentIndex);
//reference_frame
reference_frame = "";
piecesize = BitConverter.ToInt32(SERIALIZEDSTUFF, currentIndex);
currentIndex += 4;
reference_frame = Encoding.ASCII.GetString(SERIALIZEDSTUFF, currentIndex, piecesize);
currentIndex += piecesize;
}
public override void Randomize()
{
int arraylength = -1;
Random rand = new Random();
int strlength;
byte[] strbuf, myByte;
//header
header = new Header();
header.Randomize();
//pose
pose = new Pose();
pose.Randomize();
//twist
twist = new Twist();
twist.Randomize();
//success
success = rand.Next(2) == 1;
//status_message
strlength = rand.Next(100) + 1;
strbuf = new byte[strlength];
rand.NextBytes(strbuf); //fill the whole buffer with random bytes
for (int __x__ = 0; __x__ < strlength; __x__++)
{
if (strbuf[__x__] == 0) //replace null chars with non-null random ones
{
strbuf[__x__] = (byte)(rand.Next(254) + 1);
}
}
strbuf[strlength - 1] = 0; //null terminate
status_message = Encoding.ASCII.GetString(strbuf);
}
protected override bool Loop()
{
Twist twist = new Twist();
twist.Linear.X = 1f;
Input = twist;
Mat cam1 = cameraThread.Image1;
if (cam1 != null)
{
Console.WriteLine("Camera 1: {0} x {1}", cam1.Width, cam1.Height);
}
if (!Sleep(1000))
{
return(false);
}
twist.Linear.X = 0f;
Input = twist;
Mat cam2 = cameraThread.Image2;
if (cam2 != null)
{
Console.WriteLine("Camera 2: {0} x {1}", cam2.Width, cam2.Height);
}
return(Sleep(1000));
}
public TwistVel(Twist p) : this(kdlPINVOKE.new_TwistVel__SWIG_3(Twist.getCPtr(p)), true)
{
if (kdlPINVOKE.SWIGPendingException.Pending)
{
throw kdlPINVOKE.SWIGPendingException.Retrieve();
}
}
public static TwistSwing Decompose(CartesianAxis twistAxis, Quaternion q)
{
DebugUtilities.AssertIsUnit(q);
float w = q.W;
float x = q[(int)twistAxis];
float y = q[((int)twistAxis + 1) % 3];
float z = q[((int)twistAxis + 2) % 3];
float swingW = (float)Sqrt(Sqr(w) + Sqr(x));
float twistW, twistZ;
if (swingW != 0)
{
twistW = w / swingW;
twistZ = x / swingW;
}
else
{
//if swingW is 0, then there isn't a unique decomposition, so I'll arbitrarily assume no twist
twistW = 1;
twistZ = 0;
}
float swingY = twistW * y - twistZ * z;
float swingZ = twistW * z + twistZ * y;
var twist = new Twist(Sign(twistW) * twistZ);
var swing = new Swing(swingY, swingZ);
return(new TwistSwing(twist, swing));
}
public LinkState(string link_name, Pose pose, Twist twist, string reference_frame)
{
this.link_name = link_name;
this.pose = pose;
this.twist = twist;
this.reference_frame = reference_frame;
}
public void setBodyTwist(Twist body_twist)
{
lock ( mutex_ )
{
UnityEngine.Quaternion orientation = new UnityEngine.Quaternion((float)orientation_.x, (float)orientation_.y, (float)orientation_.z, (float)orientation_.w);
orientation = UnityEngine.Quaternion.Inverse(orientation);
UnityEngine.Vector3 body_linear = new UnityEngine.Vector3((float)body_twist.linear.x, (float)body_twist.linear.y, (float)body_twist.linear.z);
UnityEngine.Vector3 world_linear = orientation * body_linear;
// Eigen::Quaterniond orientation(orientation_.w, orientation_.x, orientation_.y, orientation_.z);
// Eigen::Matrix<double,3,3> inverse_rotation_matrix(orientation.inverse().toRotationMatrix());
//
// Eigen::Vector3d body_linear(body_twist.linear.x, body_twist.linear.y, body_twist.linear.z);
// Eigen::Vector3d world_linear(inverse_rotation_matrix * body_linear);
twist_.linear.x = world_linear.x;
twist_.linear.y = world_linear.y;
twist_.linear.z = world_linear.z;
UnityEngine.Vector3 body_angular = new UnityEngine.Vector3((float)body_twist.angular.x, (float)body_twist.angular.y, (float)body_twist.angular.z);
UnityEngine.Vector3 world_angular = orientation * body_angular;
// Eigen::Vector3d body_angular(body_twist.angular.x, body_twist.angular.y, body_twist.angular.z);
// Eigen::Vector3d world_angular(inverse_rotation_matrix * body_angular);
twist_.angular.x = world_angular.x;
twist_.angular.y = world_angular.y;
twist_.angular.z = world_angular.z;
}
}
public override int GetHashCode()
{
unchecked
{
int hashCode = (int)EventType;
hashCode = (hashCode * 397) ^ (int)DeviceType;
hashCode = (hashCode * 397) ^ (int)PointerId;
hashCode = (hashCode * 397) ^ Position.GetHashCode();
hashCode = (hashCode * 397) ^ Timestamp.GetHashCode();
hashCode = (hashCode * 397) ^ (int)KeyModifiers;
hashCode = (hashCode * 397) ^ ContactRect.GetHashCode();
hashCode = (hashCode * 397) ^ IsBarrelButtonPressed.GetHashCode();
hashCode = (hashCode * 397) ^ IsCanceled.GetHashCode();
hashCode = (hashCode * 397) ^ IsEraser.GetHashCode();
hashCode = (hashCode * 397) ^ IsHorizontalMouseWheel.GetHashCode();
hashCode = (hashCode * 397) ^ IsInRange.GetHashCode();
hashCode = (hashCode * 397) ^ IsInverted.GetHashCode();
hashCode = (hashCode * 397) ^ IsLeftButtonPressed.GetHashCode();
hashCode = (hashCode * 397) ^ IsMiddleButtonPressed.GetHashCode();
hashCode = (hashCode * 397) ^ IsRightButtonPressed.GetHashCode();
hashCode = (hashCode * 397) ^ IsXButton1Pressed.GetHashCode();
hashCode = (hashCode * 397) ^ IsXButton2Pressed.GetHashCode();
hashCode = (hashCode * 397) ^ IsPrimary.GetHashCode();
hashCode = (hashCode * 397) ^ MouseWheelDelta;
hashCode = (hashCode * 397) ^ Orientation.GetHashCode();
hashCode = (hashCode * 397) ^ TouchConfidence.GetHashCode();
hashCode = (hashCode * 397) ^ Twist.GetHashCode();
hashCode = (hashCode * 397) ^ XTilt.GetHashCode();
hashCode = (hashCode * 397) ^ YTilt.GetHashCode();
hashCode = (hashCode * 397) ^ (int)PointerUpdateKind;
return(hashCode);
}
}
// set pose/twist relative to the frame of this entity (Body/Model)
// leave empty or "world" or "map" defaults to world-frame
public ModelState()
{
this.model_name = "";
this.pose = new Pose();
this.twist = new Twist();
this.reference_frame = "";
}
public ModelState(string model_name, Pose pose, Twist twist, string reference_frame)
{
this.model_name = model_name;
this.pose = pose;
this.twist = twist;
this.reference_frame = reference_frame;
}
// set pose/twist relative to the frame of this link/body
// leave empty or "world" or "map" defaults to world-frame
public LinkState()
{
this.link_name = "";
this.pose = new Pose();
this.twist = new Twist();
this.reference_frame = "";
}
public void TestAsQuaternion()
{
var twist = new Twist(0.4f);
var swing = new Swing(0.2f, 0.3f);
var twistSwing = new TwistSwing(twist, swing);
MathAssert.AreEqual(
twist.AsQuaternion(CartesianAxis.X).Chain(swing.AsQuaternion(CartesianAxis.X)),
twistSwing.AsQuaternion(CartesianAxis.X),
Acc);
MathAssert.AreEqual(
twist.AsQuaternion(CartesianAxis.X).Chain(swing.AsQuaternion(CartesianAxis.X)),
twistSwing.AsQuaternion(CartesianAxis.X),
Acc);
MathAssert.AreEqual(
twist.AsQuaternion(CartesianAxis.X).Chain(swing.AsQuaternion(CartesianAxis.X)),
twistSwing.AsQuaternion(CartesianAxis.X),
Acc);
MathAssert.AreEqual(
twist.AsQuaternion(CartesianAxis.X).Chain(swing.AsQuaternion(CartesianAxis.X)),
twistSwing.AsQuaternion(CartesianAxis.X),
Acc);
}
//[System.Diagnostics.DebuggerStepThrough]
public override void Deserialize(byte[] SERIALIZEDSTUFF, ref int currentIndex)
{
int arraylength = -1;
bool hasmetacomponents = false;
int piecesize = 0;
byte[] thischunk, scratch1, scratch2;
IntPtr h;
object __thing;
//header
header = new Header(SERIALIZEDSTUFF, ref currentIndex);
//pose
pose = new Pose(SERIALIZEDSTUFF, ref currentIndex);
//twist
twist = new Twist(SERIALIZEDSTUFF, ref currentIndex);
//success
success = SERIALIZEDSTUFF[currentIndex++] == 1;
//status_message
status_message = "";
piecesize = BitConverter.ToInt32(SERIALIZEDSTUFF, currentIndex);
currentIndex += 4;
status_message = Encoding.ASCII.GetString(SERIALIZEDSTUFF, currentIndex, piecesize);
currentIndex += piecesize;
}
private Twist TranslateTwist()
{
Twist twist = new Twist();
twist.Linear.X = controller.LeftThumbstick.Y;
return(twist);
}
public bool update (Twist command)
{
if ( !connected () )
return false;
command.linear.z = get ();
return true;
}
public bool update (Twist command)
{
if ( !connected () )
return false;
getCommand ( ref command.linear.x, ref command.linear.y );
return true;
}
public void setTwist(Twist twist)
{
lock ( mutex_ )
{
twist_ = twist;
}
}
public static Twist Twist(Random rnd)
{
var twistX = ContinuousUniform.Sample(rnd, -1, 1);
var twist = new Twist((float)twistX);
return(twist);
}
public void setColumn(uint i, Twist t)
{
kdlPINVOKE.Jacobian_setColumn(swigCPtr, i, Twist.getCPtr(t));
if (kdlPINVOKE.SWIGPendingException.Pending)
{
throw kdlPINVOKE.SWIGPendingException.Retrieve();
}
}
/// <summary>
/// Extends the type <c>Twist</c> with a method <c>ToTwistMessage</c>, which creates an twist message.
/// </summary>
/// <param name="twist">Twist</c> object to be extended</param>
/// <returns>Returns an instance of <c>geometry_msgs.Twist</c>.</returns>
public static geometry_msgs.Twist ToTwistMessage(this Twist twist)
{
return(new geometry_msgs.Twist()
{
linear = twist.Linear.ToVector3Message(),
angular = twist.Angular.ToVector3Message()
});
}
public OdomSubscriberHelper (NodeHandle nh, string topic, Pose pose, Twist twist, Accel acceleration, Header header)
{
pose_ = pose;
twist_ = twist;
acceleration_ = acceleration;
header_ = header;
odom_sub_ = nh.subscribe<Odometry> ( topic, 1, odomCallback );
}
public GetModelStateResponse(Header header, Pose pose, Twist twist, bool success, string status_message)
{
this.header = header;
this.pose = pose;
this.twist = twist;
this.success = success;
this.status_message = status_message;
}
private void TestClampRotation(Vector3 input, Vector3 expected)
{
var inputTS = new TwistSwing(Twist.MakeFromAngle(input.X), Swing.MakeFromAxisAngleProduct(input.Y, input.Z));
var clampedTS = constraint.Clamp(inputTS);
var expectedTS = new TwistSwing(Twist.MakeFromAngle(expected.X), Swing.MakeFromAxisAngleProduct(expected.Y, expected.Z));
MathAssert.AreEqual(expectedTS, clampedTS, 1e-4f);
}
public void TestCalculateDelta()
{
var initialTwist = Twist.MakeFromAngle(0.2f);
var finalTwist = Twist.MakeFromAngle(0.8f);
var deltaTwist = Twist.CalculateDelta(initialTwist, finalTwist);
MathAssert.AreEqual(Twist.MakeFromAngle(0.6f), deltaTwist, Acc);
}
// comments if available
public GetModelStateResponse()
{
this.header = new Header();
this.pose = new Pose();
this.twist = new Twist();
this.success = false;
this.status_message = "";
}
private void SendStopMessage()
{
Twist message = new Twist();
message.linear = stopLinear;
message.angular = stopAngular;
cmdPublisher.Publish(message);
}
public void TestApplyDelta()
{
var initialTwist = Twist.MakeFromAngle(0.5f);
var deltaTwist = Twist.MakeFromAngle(0.3f);
var finalTwist = Twist.ApplyDelta(initialTwist, deltaTwist);
MathAssert.AreEqual(Twist.MakeFromAngle(0.8f), finalTwist, Acc);
}
