public static SliderJoint CreateSliderJoint(World world, Body bodyA, Body bodyB, Vector2 anchorA, Vector2 anchorB, float minLength, float maxLength)
{
SliderJoint sliderJoint = new SliderJoint(bodyA, bodyB, anchorA, anchorB, minLength, maxLength);
world.AddJoint(sliderJoint);
return(sliderJoint);
}
/// <summary>
/// Creates GUI elements for fields specific to the slider joint.
/// </summary>
protected void BuildGUI(SliderJoint joint)
{
enableLimitField.OnChanged += x =>
{
joint.SetFlag(SliderJointFlag.Limit, x);
MarkAsModified();
ConfirmModify();
ToggleLimitFields(x);
};
Layout.AddElement(enableLimitField);
limitLayout = Layout.AddLayoutX();
{
limitLayout.AddSpace(10);
GUILayoutY limitContentsLayout = limitLayout.AddLayoutY();
limitGUI = new LimitLinearRangeGUI(joint.Limit, limitContentsLayout, Persistent);
limitGUI.OnChanged += (x, y) =>
{
joint.Limit = x;
joint.Limit.SetBase(y);
MarkAsModified();
};
limitGUI.OnConfirmed += ConfirmModify;
}
ToggleLimitFields(joint.HasFlag(SliderJointFlag.Limit));
base.BuildGUI(joint, true);
}
/// <summary>
/// Attaches the bodies with revolute joints.
/// </summary>
/// <param name="world">The world.</param>
/// <param name="bodies">The bodies.</param>
/// <param name="localAnchorA">The local anchor A.</param>
/// <param name="localAnchorB">The local anchor B.</param>
/// <param name="connectFirstAndLast">if set to <c>true</c> [connect first and last].</param>
/// <param name="collideConnected">if set to <c>true</c> [collide connected].</param>
/// <param name="minLength">Minimum length of the slider joint.</param>
/// <param name="maxLength">Maximum length of the slider joint.</param>
/// <returns></returns>
public static List <SliderJoint> AttachBodiesWithSliderJoint(World world, List <Body> bodies, Vector2 localAnchorA,
Vector2 localAnchorB, bool connectFirstAndLast,
bool collideConnected, float minLength,
float maxLength)
{
List <SliderJoint> joints = new List <SliderJoint>(bodies.Count + 1);
for (int i = 1; i < bodies.Count; i++)
{
SliderJoint joint = new SliderJoint(bodies[i], bodies[i - 1], localAnchorA, localAnchorB, minLength,
maxLength);
joint.CollideConnected = collideConnected;
world.AddJoint(joint);
joints.Add(joint);
}
if (connectFirstAndLast)
{
SliderJoint lastjoint = new SliderJoint(bodies[0], bodies[bodies.Count - 1], localAnchorA, localAnchorB,
minLength, maxLength);
lastjoint.CollideConnected = collideConnected;
world.AddJoint(lastjoint);
joints.Add(lastjoint);
}
return(joints);
}
public SliderJoint CreateSliderJoint(Body body1, Vector2 anchor1, Body body2, Vector2 anchor2, float min,
float max)
{
SliderJoint sliderJoint = new SliderJoint(body1, anchor1, body2, anchor2, min, max);
return(sliderJoint);
}
private static void DrawSliderJoint(SliderJoint joint)
{
Vector3 anchor = GetAnchor(joint, JointBody.Anchor);
Vector3 target = GetAnchor(joint, JointBody.Target);
Vector3 normal = -joint.SceneObject.Right;
if (joint.HasFlag(SliderJointFlag.Limit))
{
LimitLinearRange limit = joint.Limit;
float max = MathEx.Min(10000.0f, limit.upper);
float min = MathEx.Clamp(limit.lower, 0.0f, max);
max = MathEx.Max(max, min);
Gizmos.Color = Color.Red;
Gizmos.DrawLine(anchor, anchor + normal * min);
Gizmos.Color = Color.Green;
Gizmos.DrawLine(anchor + normal * min, anchor + normal * max);
}
else
{
Gizmos.Color = Color.Green;
float length = 100.0f;
Gizmos.DrawLine(anchor, anchor + normal * length);
}
Gizmos.Color = Color.Yellow;
Gizmos.DrawSphere(target, 0.05f);
}
public SliderJoint CreateSliderJoint(PhysicsSimulator physicsSimulator, Body body1, Vector2 anchor1, Body body2,
Vector2 anchor2, float min, float max)
{
SliderJoint sliderJoint = CreateSliderJoint(body1, anchor1, body2, anchor2, min, max);
physicsSimulator.Add(sliderJoint);
return(sliderJoint);
}
/// <inheritdoc/>
protected internal override void Initialize()
{
SliderJoint joint = InspectedObject as SliderJoint;
if (joint != null)
{
BuildGUI(joint);
}
}
/// <inheritdoc/>
protected internal override void Initialize()
{
SliderJoint joint = (SliderJoint)InspectedObject;
BuildGUI(joint, true);
drawer.AddDefault(joint, typeof(SliderJoint));
drawer.AddField("Enable limit",
() => joint.HasFlag(SliderJointFlag.Limit),
x => joint.SetFlag(SliderJointFlag.Limit, x));
drawer.AddConditional("Limit", () => joint.HasFlag(SliderJointFlag.Limit));
}
/// <summary>
/// Updates all GUI elements from current values in the joint.
/// </summary>
/// <param name="joint">Joint to update the GUI from.</param>
protected void Refresh(SliderJoint joint)
{
if (enableLimitField.Value != joint.EnableLimit)
{
enableLimitField.Value = joint.EnableLimit;
ToggleLimitFields(joint.EnableLimit);
}
limitGUI.Limit = joint.Limit;
base.Refresh(joint);
}
private void DrawSliderJoints(SpriteBatch spriteBatch)
{
for (int i = 0; i < _physicsSimulator.JointList.Count; i++)
{
if (_physicsSimulator.JointList[i] is SliderJoint)
{
SliderJoint sliderJoint = (SliderJoint)_physicsSimulator.JointList[i];
_sliderJointRectangleBrush.Draw(spriteBatch, sliderJoint.WorldAnchor1);
_sliderJointRectangleBrush.Draw(spriteBatch, sliderJoint.WorldAnchor2);
_sliderJointLineBrush.Draw(spriteBatch, sliderJoint.WorldAnchor1, sliderJoint.WorldAnchor2);
}
}
}
/// <summary>
/// Updates all GUI elements from current values in the joint.
/// </summary>
/// <param name="joint">Joint to update the GUI from.</param>
protected void Refresh(SliderJoint joint)
{
bool enableLimit = joint.HasFlag(SliderJointFlag.Limit);
if (enableLimitField.Value != enableLimit)
{
enableLimitField.Value = enableLimit;
ToggleLimitFields(enableLimit);
}
limitGUI.Limit = joint.Limit;
base.Refresh(joint);
}
internal override void UpdateJoint()
{
base.UpdateJoint();
if (this.joint == null)
{
return;
}
SliderJoint j = this.joint as SliderJoint;
j.LocalAnchorB = GetFarseerPoint(this.OtherBody, this.localAnchorB);
j.LocalAnchorA = GetFarseerPoint(this.ParentBody, this.localAnchorA);
j.MaxLength = PhysicsUnit.LengthToPhysical * this.maxLength;
j.MinLength = PhysicsUnit.LengthToPhysical * this.minLength;
}
internal override void UpdateJoint()
{
base.UpdateJoint();
if (this.joint == null)
{
return;
}
SliderJoint j = this.joint as SliderJoint;
j.LocalAnchorB = GetFarseerPoint(this.BodyB, this.localAnchorB);
j.LocalAnchorA = GetFarseerPoint(this.BodyA, this.localAnchorA);
j.MaxLength = PhysicsConvert.ToPhysicalUnit(this.maxLength);
j.MinLength = PhysicsConvert.ToPhysicalUnit(this.minLength);
}
private SliderJointTest()
{
BodyFactory.CreateEdge(World, new Vector2(-40, 0), new Vector2(40, 0));
Body fA = BodyFactory.CreateRectangle(World, 4, 4, 1, new Vector2(-5, 4));
fA.BodyType = BodyType.Dynamic;
Body fB = BodyFactory.CreateRectangle(World, 4, 4, 1, new Vector2(5, 4));
fB.BodyType = BodyType.Dynamic;
SliderJoint joint = new SliderJoint(fA, fB, Vector2.Zero, Vector2.Zero, 5, 10);
World.AddJoint(joint);
}
/// <inheritdoc/>
protected internal override InspectableState Refresh()
{
SliderJoint joint = InspectedObject as SliderJoint;
if (joint == null)
{
return(InspectableState.NotModified);
}
Refresh(joint);
InspectableState oldState = modifyState;
if (modifyState.HasFlag(InspectableState.Modified))
{
modifyState = InspectableState.NotModified;
}
return(oldState);
}
public Joint GetPhysicsJoint(Vector2 scale)
{
float scaleD = (scale.X + scale.Y) / 2f;
Body body1, body2;
string[] bodyNames = PrefabUtils.GetEntityAndComponentName(Body1);
if (bodyNames[0] == null)
{
body1 = FatherEntity.PhysicsEntity.GetBody(Body1);
}
else
{
body1 = GameWorldManager.Instance.GetEntity(bodyNames[0]).PhysicsEntity.GetBody(bodyNames[1]);
}
bodyNames = PrefabUtils.GetEntityAndComponentName(Body2);
if (bodyNames[0] == null)
{
body2 = FatherEntity.PhysicsEntity.GetBody(Body2);
}
else
{
body2 = GameWorldManager.Instance.GetEntity(bodyNames[0]).PhysicsEntity.GetBody(bodyNames[1]);
}
switch (Type)
{
case JointType.Line:
LineJoint joint = new LineJoint(body1, body2, UnitsConverter.ToSimUnits(Anchor) * scale, Axis);
joint.MaxMotorTorque = MaxMotorTorque;
joint.MotorEnabled = MotorEnabled;
joint.Frequency = Frequency;
joint.DampingRatio = DampingRatio;
joint.CollideConnected = CollideConnected;
Platform.Instance.PhysicsWorld.AddJoint(joint);
return(joint);
case JointType.Revolute:
RevoluteJoint joint2 = new RevoluteJoint(body1, body2, UnitsConverter.ToSimUnits(Anchor) * scale, UnitsConverter.ToSimUnits(Anchor2) * scale);
joint2.CollideConnected = CollideConnected;
joint2.MaxMotorTorque = MaxMotorTorque;
joint2.MotorEnabled = MotorEnabled;
Platform.Instance.PhysicsWorld.AddJoint(joint2);
return(joint2);
case JointType.Weld:
WeldJoint joint3 = new WeldJoint(body1, body2, UnitsConverter.ToSimUnits(Anchor) * scale, UnitsConverter.ToSimUnits(Anchor2) * scale);
joint3.CollideConnected = CollideConnected;
Platform.Instance.PhysicsWorld.AddJoint(joint3);
return(joint3);
case JointType.Slider:
SliderJoint joint4 = new SliderJoint(body1, body2, UnitsConverter.ToSimUnits(Anchor) * scale, UnitsConverter.ToSimUnits(Anchor2) * scale, UnitsConverter.ToSimUnits(MinLength) * scaleD, UnitsConverter.ToSimUnits(MaxLength) * scaleD);
joint4.DampingRatio = DampingRatio;
joint4.CollideConnected = CollideConnected;
Platform.Instance.PhysicsWorld.AddJoint(joint4);
return(joint4);
case JointType.Distance:
DistanceJoint joint5 = new DistanceJoint(body1, body2, UnitsConverter.ToSimUnits(Anchor) * scale, UnitsConverter.ToSimUnits(Anchor2) * scale);
joint5.Frequency = Frequency;
joint5.DampingRatio = DampingRatio;
joint5.CollideConnected = CollideConnected;
joint5.Length = UnitsConverter.ToSimUnits(Length) * scaleD;
Platform.Instance.PhysicsWorld.AddJoint(joint5);
return(joint5);
case JointType.Prismatic:
PrismaticJoint joint6 = new PrismaticJoint(body1, body2, UnitsConverter.ToSimUnits(Anchor) * scale, UnitsConverter.ToSimUnits(Anchor2) * scale, Axis);
joint6.CollideConnected = CollideConnected;
joint6.UpperLimit = UnitsConverter.ToSimUnits(UpperLimit) * scaleD;
joint6.LowerLimit = UnitsConverter.ToSimUnits(LowerLimit) * scaleD;
joint6.LimitEnabled = LimitEnabled;
joint6.MotorEnabled = MotorEnabled;
joint6.MaxMotorForce = MaxMotorForce * scaleD;
joint6.MotorSpeed = MotorSpeed * scaleD;
Platform.Instance.PhysicsWorld.AddJoint(joint6);
return(joint6);
default:
return(null);
}
}
public ICollisionJoint[] LoadSimulationJoints(
ICollisionShape[] objects)
{
var xmlDoc = new XmlDocument();
xmlDoc.Load(FileNameObjectProperties);
XmlNodeList xmlList = xmlDoc.SelectNodes(nodePathJoints);
ICollisionJoint[] joints = new ICollisionJoint[xmlList.Count];
for (int i = 0; i < xmlList.Count; i++)
{
//Object index A
int indexA = Convert.ToInt32(xmlList[i][objectIndexAAttribute].InnerText);
//Object index B
int indexB = Convert.ToInt32(xmlList[i][objectIndexBAttribute].InnerText);
XmlNodeList jointPropertiesList = xmlList[i].SelectNodes(jointProperties);
ICollisionJoint[] joint = new ICollisionJoint[jointPropertiesList.Count];
for (int j = 0; j < jointPropertiesList.Count; j++)
{
//Joint type
var jointType = (JointType)Convert.ToInt32(jointPropertiesList[j][this.jointType].InnerText);
//Restore coefficient
double K = Convert.ToDouble(jointPropertiesList[j][restoreCoeffAttribute].InnerText);
//Stretch coefficient
double C = Convert.ToDouble(jointPropertiesList[j][stretchCoeffAttribute].InnerText);
//Position
var startAnchorPosition = new Vector3d(
Convert.ToDouble(jointPropertiesList[j][positionJointAttribute].Attributes["x"].Value),
Convert.ToDouble(jointPropertiesList[j][positionJointAttribute].Attributes["y"].Value),
Convert.ToDouble(jointPropertiesList[j][positionJointAttribute].Attributes["z"].Value));
//Action Axis
var actionAxis = new Vector3d(
Convert.ToDouble(jointPropertiesList[j][this.actionAxis].Attributes["x"].Value),
Convert.ToDouble(jointPropertiesList[j][this.actionAxis].Attributes["y"].Value),
Convert.ToDouble(jointPropertiesList[j][this.actionAxis].Attributes["z"].Value));
switch (jointType)
{
case JointType.Fixed:
joint [j] = new FixedJoint(
objects[indexA],
objects[indexB],
K,
C);
break;
case JointType.BallAndSocket:
joint[j] = new BallAndSocketJoint(
objects[indexA],
objects[indexB],
startAnchorPosition,
K,
C);
break;
case JointType.Slider:
joint[j] = new SliderJoint(
objects[indexA],
objects[indexB],
startAnchorPosition,
actionAxis,
K,
C);
joint[j].SetLinearLimit(Convert.ToDouble(jointPropertiesList[j][linearLimitMin].InnerText), Convert.ToDouble(jointPropertiesList[j][linearLimitMax].InnerText));
break;
case JointType.Piston:
joint[j] = new PistonJoint(
objects[indexA],
objects[indexB],
startAnchorPosition,
actionAxis,
K,
C);
joint[j].SetAxis1AngularLimit(
Convert.ToDouble(jointPropertiesList[j][angularLimitMin].InnerText),
Convert.ToDouble(jointPropertiesList[j][angularLimitMax].InnerText));
joint[j].SetLinearLimit(
Convert.ToDouble(jointPropertiesList[j][linearLimitMin].InnerText),
Convert.ToDouble(jointPropertiesList[j][linearLimitMax].InnerText));
break;
case JointType.Hinge:
joint[j] = new HingeJoint(
objects[indexA],
objects[indexB],
startAnchorPosition,
actionAxis,
K,
C);
joint[j].SetAxis1AngularLimit(
Convert.ToDouble(jointPropertiesList[j][angularLimitMin].InnerText),
Convert.ToDouble(jointPropertiesList[j][angularLimitMax].InnerText));
joint[j].SetAxis1Motor(3.0, 0.15);
break;
case JointType.Universal:
joint[j] = new UniversalJoint(
objects[indexA],
objects[indexB],
startAnchorPosition,
actionAxis,
new Vector3d(1.0, 0.0, 0.0),
K,
C);
joint[j].SetAxis1AngularLimit(
Convert.ToDouble(jointPropertiesList[j][angularLimitMin].InnerText),
Convert.ToDouble(jointPropertiesList[j][angularLimitMax].InnerText));
joint[j].SetAxis2AngularLimit(
Convert.ToDouble(jointPropertiesList[j][angularLimitMin].InnerText),
Convert.ToDouble(jointPropertiesList[j][angularLimitMax].InnerText));
break;
case JointType.Hinge2:
joint[j] = new Hinge2Joint(
objects[indexA],
objects[indexB],
startAnchorPosition,
actionAxis,
new Vector3d(1.0, 0.0, 0.0),
K,
1.0,
C);
joint[j].SetAxis1AngularLimit(
Convert.ToDouble(jointPropertiesList[j][angularLimitMin].InnerText),
Convert.ToDouble(jointPropertiesList[j][angularLimitMax].InnerText));
//joint[j].SetAxis2Motor(4.0, 3.0);
break;
case JointType.Angular:
joint[j] = new AngularJoint(
objects[indexA],
objects[indexB],
startAnchorPosition,
new Vector3d(1.0, 0.0, 0.0),
new Vector3d(0.0, 1.0, 0.0),
0.16,
0.008,
0.008);
break;
}
joints[i] = joint[j];
}
}
return(joints);
}
public static void AddRollingBeats(Scene scene, TransformNode parentTrans)
{
Vector3 size = new Vector3(10.0f, 0.25f, 0.25f);
Vector3 location = new Vector3(0, 3, 3);
GeometryNode bar;
// //////////////////////////////////////////////////////////////////
//
// Create a bar and attach it to the world with a hinge with limits
//
// //////////////////////////////////////////////////////////////////
{
TransformNode pileTrans = new TransformNode();
pileTrans.Translation = location;
pileTrans.Rotation = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, MathHelper.PiOver2);
Material barMat = new Material();
barMat.Diffuse = Color.Purple.ToVector4();
barMat.Specular = Color.White.ToVector4();
barMat.SpecularPower = 10;
bar = new GeometryNode("Bar");
bar.Model = new Cylinder(size.Y, size.Y, size.X, 20);
bar.Material = barMat;
bar.Model.ShadowAttribute = ShadowAttribute.ReceiveCast;
bar.AddToPhysicsEngine = true;
bar.Physics.Interactable = true;
bar.Physics.Collidable = true;
bar.Physics.Shape = ShapeType.Cylinder;
bar.Physics.Mass = 2.0f;
parentTrans.AddChild(pileTrans);
pileTrans.AddChild(bar);
Vector3 pivot = location + parentTrans.Translation;
Vector3 pin = Vector3.UnitY;
pivot.X -= size.X * 0.5f;
HingeJoint joint = new HingeJoint(pivot, pin);
joint.NewtonHingeCallback = doubleDoor;
((NewtonPhysics)scene.PhysicsEngine).CreateJoint(bar.Physics, null, joint);
}
// /////////////////////////////////////////////////////////////////
//
// Add a sliding visualObject with limits
//
////////////////////////////////////////////////////////////////////
{
Vector3 beatLocation = location;
Vector3 beatSize = new Vector3(0.5f, 2.0f, 2.0f);
beatLocation.X += size.X * 0.25f;
TransformNode pileTrans = new TransformNode();
pileTrans.Translation = beatLocation;
Material beatMat = new Material();
beatMat.Diffuse = Color.Red.ToVector4();
beatMat.Specular = Color.White.ToVector4();
beatMat.SpecularPower = 10;
GeometryNode beat = new GeometryNode("Beat Slider");
beat.Model = new Box(beatSize);
beat.Material = beatMat;
beat.Model.ShadowAttribute = ShadowAttribute.ReceiveCast;
beat.AddToPhysicsEngine = true;
beat.Physics.Interactable = true;
beat.Physics.Collidable = true;
beat.Physics.Shape = ShapeType.Box;
beat.Physics.Mass = 2.0f;
parentTrans.AddChild(pileTrans);
pileTrans.AddChild(beat);
Vector3 pivot = beatLocation + parentTrans.Translation;
Vector3 pin = Vector3.UnitX;
sliderLimit.X = ((location.X - beatLocation.X) - size.X * 0.5f);
sliderLimit.Y = ((location.X - beatLocation.X) + size.X * 0.5f);
SliderJoint joint = new SliderJoint(pivot, pin);
sliderUpdate = delegate(IntPtr slider, ref Newton.NewtonHingeSliderUpdateDesc desc)
{
float distance = Newton.NewtonSliderGetJointPosit(slider);
if (distance < sliderLimit.X)
{
// if the distance is smaller than the predefine interval, stop the slider
desc.m_Accel = Newton.NewtonSliderCalculateStopAccel(slider, ref desc, sliderLimit.X);
return(1);
}
else if (distance > sliderLimit.Y)
{
// if the distance is larger than the predefine interval, stop the slider
desc.m_Accel = Newton.NewtonSliderCalculateStopAccel(slider, ref desc, sliderLimit.Y);
return(1);
}
// no action need it if the joint angle is with the limits
return(0);
};
joint.NewtonSliderCallback = sliderUpdate;
((NewtonPhysics)scene.PhysicsEngine).CreateJoint(beat.Physics, bar.Physics, joint);
}
// /////////////////////////////////////////////////////////////////
//
// Add a corkscrew visualObject with limits
//
// /////////////////////////////////////////////////////////////////
{
Vector3 beatLocation = location;
Vector3 beatSize = new Vector3(0.5f, 2.0f, 2.0f);
beatLocation.X -= size.X * 0.25f;
TransformNode pileTrans = new TransformNode();
pileTrans.Translation = beatLocation;
Material beatMat = new Material();
beatMat.Diffuse = Color.YellowGreen.ToVector4();
beatMat.Specular = Color.White.ToVector4();
beatMat.SpecularPower = 10;
GeometryNode beat = new GeometryNode("Beat Corkscrew");
beat.Model = new Box(beatSize);
beat.Material = beatMat;
beat.Model.ShadowAttribute = ShadowAttribute.ReceiveCast;
beat.AddToPhysicsEngine = true;
beat.Physics.Interactable = true;
beat.Physics.Collidable = true;
beat.Physics.Shape = ShapeType.Box;
beat.Physics.Mass = 2.0f;
parentTrans.AddChild(pileTrans);
pileTrans.AddChild(beat);
Vector3 pivot = beatLocation + parentTrans.Translation;
Vector3 pin = Vector3.UnitX;
corkscrewLimit.X = ((location.X - beatLocation.X) - size.X * 0.5f);
corkscrewLimit.Y = ((location.X - beatLocation.X) + size.X * 0.5f);
CorkscrewJoint joint = new CorkscrewJoint(pivot, pin);
corkscrewUpdate = delegate(IntPtr corkscrew, Newton.NewtonHingeSliderUpdateDesc[] desc)
{
// no action need it if the joint angle is with the limits
uint retCode = 0;
float distance = Newton.NewtonCorkscrewGetJointPosit(corkscrew);
// The first entry in NewtonHingeSliderUpdateDesc control the screw linear acceleration
if (distance < corkscrewLimit.X)
{
// if the distance is smaller than the predefine interval, stop the slider
desc[0].m_Accel = Newton.NewtonCorkscrewCalculateStopAccel(corkscrew,
ref desc[0], corkscrewLimit.X);
retCode |= 1;
}
else if (distance > corkscrewLimit.Y)
{
// if the distance is larger than the predefine interval, stop the slider
desc[0].m_Accel = Newton.NewtonCorkscrewCalculateStopAccel(corkscrew, ref
desc[0], corkscrewLimit.Y);
retCode |= 1;
}
// The second entry in NewtonHingeSliderUpdateDesc control the screw angular acceleration.
// Make s small screw motor by setting the angular acceleration of the screw axis
// We are not going to limit the angular rotation of the screw, but is we did we should
// or return code with 2
float omega = Newton.NewtonCorkscrewGetJointOmega(corkscrew);
desc[1].m_Accel = 1.5f - 0.2f * omega;
// or with 0x10 to tell newton this axis is active
retCode |= 2;
// return the code
return(retCode);
};
joint.NewtonCorkscrewCallback = corkscrewUpdate;
((NewtonPhysics)scene.PhysicsEngine).CreateJoint(beat.Physics, bar.Physics, joint);
}
// /////////////////////////////////////////////////////////////////
//
// Add a universal joint visualObject with limits
//
// /////////////////////////////////////////////////////////////////
{
Vector3 beatLocation = location;
Vector3 beatSize = new Vector3(0.5f, 2.0f, 2.0f);
beatLocation.X -= size.X * 0.45f;
TransformNode pileTrans = new TransformNode();
pileTrans.Translation = beatLocation;
Material beatMat = new Material();
beatMat.Diffuse = Color.YellowGreen.ToVector4();
beatMat.Specular = Color.White.ToVector4();
beatMat.SpecularPower = 10;
GeometryNode beat = new GeometryNode("Beat Universal");
beat.Model = new Box(beatSize);
beat.Material = beatMat;
beat.Model.ShadowAttribute = ShadowAttribute.ReceiveCast;
beat.AddToPhysicsEngine = true;
beat.Physics.Interactable = true;
beat.Physics.Collidable = true;
beat.Physics.Shape = ShapeType.Box;
beat.Physics.Mass = 2.0f;
parentTrans.AddChild(pileTrans);
pileTrans.AddChild(beat);
Vector3 pivot = beatLocation + parentTrans.Translation;
Vector3 pin0 = Vector3.UnitX;
Vector3 pin1 = Vector3.UnitY;
universalLimit.X = -30.0f * MathHelper.Pi / 180.0f;
universalLimit.Y = 30.0f * MathHelper.Pi / 180.0f;
UniversalJoint joint = new UniversalJoint(pivot, pin0, pin1);
universalUpdate = delegate(IntPtr universal, Newton.NewtonHingeSliderUpdateDesc[] desc)
{
// no action need it if the joint angle is with the limits
uint retCode = 0;
float omega = Newton.NewtonUniversalGetJointOmega0(universal);
desc[0].m_Accel = -1.5f - 0.2f * omega;
retCode |= 1;
float angle = Newton.NewtonUniversalGetJointAngle1(universal);
if (angle < universalLimit.X)
{
desc[1].m_Accel = Newton.NewtonUniversalCalculateStopAlpha1(universal, ref desc[1],
universalLimit.X);
retCode |= 2;
}
else if (angle > universalLimit.Y)
{
desc[1].m_Accel = Newton.NewtonUniversalCalculateStopAlpha1(universal, ref desc[1],
universalLimit.Y);
retCode |= 2;
}
// return the code
return(retCode);
};
joint.NewtonUniversalCallback = universalUpdate;
((NewtonPhysics)scene.PhysicsEngine).CreateJoint(beat.Physics, bar.Physics, joint);
}
}
// TODO
// * Support Urdf axis that aren't directly aligned with the
// X, Y, or Z axis
/// <summary>
/// <para>ConfigureJoint</para>
/// Configures the Godot Generic6DOFJoint
/// properties to match what the Urdf joint
/// contains.
/// </summary>
/// <param name="base_joint">Urdf Joint specifications.</param>
/// <returns>Godot joint matching specs.</returns>
private Godot.Joint ConfigureJoint(RosSharp.Urdf.Joint base_joint)
{
// The Urdf joint axis specifies the axis of rotation for revolute joints,
// axis of translation for prismatic joints, and the surface normal
// for planar joints.
// If it's not specified accessing it will error out, so we need to
// manually specify the default (X-axis).
double[] j_axis;
try
{
j_axis = base_joint.axis.xyz;
}
catch
{
j_axis = new double[] { 1.0, 0.0, 0.0 };
}
GD.Print(base_joint.name + " axis: " + j_axis[0] + " " + j_axis[1] + " " + j_axis[2]);
JointMaker mkr = new JointMaker();
// Type comments taken from https://wiki.ros.org/urdf/XML/joint
switch (base_joint.type)
{
case "revolute":
// A hinge joint that rotates along the axis and has a
// limited range specified by the upper and lower limits.
// HingeJoint revJoint = mkr.CreateHingeJoint();
// // HingeJoint revJoint = new HingeJoint();
// // revJoint.rot/
// GD.Print("setting hingejoint flags");
// revJoint.SetFlag(HingeJoint.Flag.UseLimit, true);
// revJoint.SetFlag(HingeJoint.Flag.EnableMotor, true);
// revJoint.SetParam(HingeJoint.Param.MotorTargetVelocity, 0F);
// revJoint.SetParam(HingeJoint.Param.MotorMaxImpulse, 1024F);
// revJoint.SetParam(HingeJoint.Param.Bias, 1F);
// revJoint.SetParam(HingeJoint.Param.LimitLower, (float)base_joint.limit.lower * (180F / (float)Math.PI));
// revJoint.SetParam(HingeJoint.Param.LimitUpper, (float)base_joint.limit.upper * (180F / (float)Math.PI));
// return revJoint;
case "continuous":
// a continuous hinge joint that rotates around the axis
// and has no upper and lower limits.
HingeJoint contJoint = new HingeJoint();
contJoint.SetFlag(HingeJoint.Flag.UseLimit, false);
contJoint.SetFlag(HingeJoint.Flag.EnableMotor, true);
contJoint.SetParam(HingeJoint.Param.MotorTargetVelocity, 0F);
return(contJoint);
case "prismatic":
// a sliding joint that slides along the axis, and has a
// limited range specified by the upper and lower limits.
SliderJoint slideJoint = new SliderJoint();
slideJoint.SetParam(SliderJoint.Param.LinearLimitLower, (float)base_joint.limit.lower);
slideJoint.SetParam(SliderJoint.Param.LinearLimitUpper, (float)base_joint.limit.upper);
return(slideJoint);
case "fixed":
// This is not really a joint because it cannot move.
// All degrees of freedom are locked. This type of joint
// does not require the axis, calibration, dynamics,
// limits or safety_controller.
Generic6DOFJoint pinJoint = new Generic6DOFJoint();
pinJoint.SetFlagX(Generic6DOFJoint.Flag.EnableAngularLimit, true);
pinJoint.SetFlagY(Generic6DOFJoint.Flag.EnableAngularLimit, true);
pinJoint.SetFlagZ(Generic6DOFJoint.Flag.EnableAngularLimit, true);
pinJoint.SetFlagX(Generic6DOFJoint.Flag.EnableLinearLimit, true);
pinJoint.SetFlagY(Generic6DOFJoint.Flag.EnableLinearLimit, true);
pinJoint.SetFlagZ(Generic6DOFJoint.Flag.EnableLinearLimit, true);
return(pinJoint);
case "floating":
// This joint allows motion for all 6 degrees of freedom.
Generic6DOFJoint genJoint = new Generic6DOFJoint();
genJoint.SetFlagX(Generic6DOFJoint.Flag.EnableAngularLimit, false);
genJoint.SetFlagY(Generic6DOFJoint.Flag.EnableAngularLimit, false);
genJoint.SetFlagZ(Generic6DOFJoint.Flag.EnableAngularLimit, false);
genJoint.SetFlagX(Generic6DOFJoint.Flag.EnableLinearLimit, false);
genJoint.SetFlagY(Generic6DOFJoint.Flag.EnableLinearLimit, false);
genJoint.SetFlagZ(Generic6DOFJoint.Flag.EnableLinearLimit, false);
return(genJoint);
case "planar":
// This joint allows motion in a plane perpendicular to the axis.
Generic6DOFJoint planJoint = new Generic6DOFJoint();
if (j_axis[0] == 1.0)
{
planJoint.SetParamY(
Generic6DOFJoint.Param.LinearUpperLimit,
(float)base_joint.limit.upper
);
planJoint.SetParamY(
Generic6DOFJoint.Param.LinearLowerLimit,
(float)base_joint.limit.lower
);
planJoint.SetParamZ(
Generic6DOFJoint.Param.LinearUpperLimit,
(float)base_joint.limit.upper
);
planJoint.SetParamZ(
Generic6DOFJoint.Param.LinearLowerLimit,
(float)base_joint.limit.lower
);
}
if (j_axis[1] == 1.0)
{
planJoint.SetParamY(
Generic6DOFJoint.Param.LinearUpperLimit,
(float)base_joint.limit.upper
);
planJoint.SetParamY(
Generic6DOFJoint.Param.LinearLowerLimit,
(float)base_joint.limit.lower
);
planJoint.SetParamX(
Generic6DOFJoint.Param.LinearUpperLimit,
(float)base_joint.limit.upper
);
planJoint.SetParamX(
Generic6DOFJoint.Param.LinearLowerLimit,
(float)base_joint.limit.lower
);
}
if (j_axis[2] == 1.0)
{
planJoint.SetParamX(
Generic6DOFJoint.Param.LinearUpperLimit,
(float)base_joint.limit.upper
);
planJoint.SetParamX(
Generic6DOFJoint.Param.LinearLowerLimit,
(float)base_joint.limit.lower
);
planJoint.SetParamZ(
Generic6DOFJoint.Param.LinearUpperLimit,
(float)base_joint.limit.upper
);
planJoint.SetParamZ(
Generic6DOFJoint.Param.LinearLowerLimit,
(float)base_joint.limit.lower
);
}
return(planJoint);
default:
GD.Print("testing: " + base_joint.type);
break;
}
return(null);
}
