public BasicJoint3D(IWorld world, IBody3D body1, IBody3D body2, TSVector anchorPosition, TSVector anchorPosition2) : base((World)world)
{
firstBody = body1;
secondBody = body2;
//hingeA = hingeAxis;
worldPointConstraint = new FixedPoint[2];
///hingeAxis *= FP.Half;
TSVector anchor = anchorPosition;
//TSVector.Add(ref anchor, ref hingeAxis, out anchor);
TSVector anchor2 = anchorPosition2;
//TSVector.Subtract(ref anchor2, ref hingeAxis, out anchor2);
//pointLock = new PointPointDistance((RigidBody)body1, (RigidBody)body2, position, position);
//pointLock.Distance = 0;
//pointLock.Behavior = PointPointDistance.DistanceBehavior.LimitMaximumDistance;
worldPointConstraint[0] = new FixedPoint((RigidBody)body1, (RigidBody)body2, anchor, anchor);
worldPointConstraint[1] = new FixedPoint((RigidBody)body2, (RigidBody)body1, anchor, anchor);
StateTracker.AddTracking(worldPointConstraint[0]);
//StateTracker.AddTracking(worldPointConstraint[1]);
Activate();
}
public CharacterJoint3D(IWorld world, IBody3D body1, IBody3D body2, TSVector position, TSVector hingeAxis) : base((World)world)
{
RigidBody rigid1 = (RigidBody)body1;
RigidBody rigid2 = (RigidBody)body2;
fixedAngle = new FixedAngle(rigid1, rigid2);
pointOnLine = new PointOnLine(rigid1, rigid2, rigid1.position, rigid2.position);
firstBody = body1;
secondBody = body2;
hingeA = hingeAxis;
worldPointConstraint = new PointOnPoint[2];
hingeAxis *= FP.Half;
TSVector anchor = position;
TSVector.Add(ref anchor, ref hingeAxis, out anchor);
TSVector anchor2 = position;
TSVector.Subtract(ref anchor2, ref hingeAxis, out anchor2);
worldPointConstraint[0] = new PointOnPoint((RigidBody)body1, (RigidBody)body2, anchor);
worldPointConstraint[1] = new PointOnPoint((RigidBody)body2, (RigidBody)body1, anchor2);
StateTracker.AddTracking(worldPointConstraint[0]);
StateTracker.AddTracking(worldPointConstraint[1]);
//UnityEngine.CharacterJoint;
Activate();
}
public override void OnSyncedStart()
{
thisBody = GetComponent <TSRigidBody>();
IBody3D body1 = GetComponent <TSCollider>().Body;
IBody3D body2 = connectedBody.Body;
Vector3 worldPos = transform.TransformPoint(anchor);
TSVector TSworldPos = worldPos.ToTSVector();
Vector3 worldAxis = transform.TransformDirection(Axis);
TSWorldAxis = worldAxis.ToTSVector();
if (useLimits)
{
thisJoint = new LimitedHingeJoint(PhysicsWorldManager.instance.GetWorld(), body1, body2, TSworldPos, TSWorldAxis, -Limits.Min, Limits.Max);
}
else
{
thisJoint = new MyHingeJoint(PhysicsWorldManager.instance.GetWorld(), body1, body2, TSworldPos, TSWorldAxis);
}
thisJoint.Activate();
}
public override void OnSyncedStart()
{
thisBody = GetComponent <TSRigidBody> ();
IBody3D body1 = GetComponent <TSCollider> ().Body;
IBody3D body2 = connectedBody.Body;
thisJoint = new BasicJoint3D(PhysicsWorldManager.instance.GetWorld(), body1, body2, transform.TransformPoint(anchor).ToTSVector(),
transform.TransformPoint(connectedAnchor).ToTSVector());
}
private HingeJoint3D(IWorld world, IBody3D body1, IBody3D body2, TSVector position, TSVector hingeAxis) : base((World)world)
{
worldPointConstraint = new PointOnPoint[2];
hingeAxis *= FP.Half;
TSVector pos1 = position; TSVector.Add(ref pos1, ref hingeAxis, out pos1);
TSVector pos2 = position; TSVector.Subtract(ref pos2, ref hingeAxis, out pos2);
worldPointConstraint[0] = new PointOnPoint((RigidBody)body1, (RigidBody)body2, pos1);
worldPointConstraint[1] = new PointOnPoint((RigidBody)body1, (RigidBody)body2, pos2);
Activate();
}
public MyHingeJoint(IWorld world, IBody3D body1, IBody3D body2, TSVector position, TSVector hingeAxis) : base((World)world)
{
firstBody = body1;
secondBody = body2;
hingeA = hingeAxis;
worldPointConstraint = new PointOnPoint[2];
hingeAxis *= FP.Half;
TSVector anchor = position;
TSVector.Add(ref anchor, ref hingeAxis, out anchor);
TSVector anchor2 = position;
TSVector.Subtract(ref anchor2, ref hingeAxis, out anchor2);
worldPointConstraint[0] = new PointOnPoint((RigidBody)body1, (RigidBody)body2, anchor);
worldPointConstraint[1] = new PointOnPoint((RigidBody)body1, (RigidBody)body2, anchor2);
StateTracker.AddTracking(worldPointConstraint[0]);
StateTracker.AddTracking(worldPointConstraint[1]);
}
public override void OnSyncedStart()
{
base.OnSyncedStart();
thisBody = GetComponent <TSRigidBody>();
IBody3D body1 = GetComponent <TSCollider>().Body;
IBody3D body2 = connectedBody.Body;
Vector3 worldPos = transform.TransformPoint(anchor);
TSVector TSworldPos = worldPos.ToTSVector();
Vector3 worldAxis = transform.TransformDirection(Axis);
TSWorldAxis = worldAxis.ToTSVector();
thisJoint = new PrismaticJoint3D(PhysicsWorldManager.instance.GetWorld(), body1, body2, minimumDistance, maximumDistance);
}
public PrismaticJoint3D(IWorld world, IBody3D body1, IBody3D body2, FP minimumDistance, FP maximumDistance)
: base((World)world)
{
//fixedAngle = new FixedAngle((RigidBody)body1, (RigidBody)body2);
pointOnLine = new PointOnLine((RigidBody)body1, (RigidBody)body2, ((RigidBody)body1).position, ((RigidBody)body2).position);
//minDistance = new PointPointDistance((RigidBody)body1, (RigidBody)body2, ((RigidBody)body1).position, ((RigidBody)body2).position);
//minDistance.Behavior = PointPointDistance.DistanceBehavior.LimitMinimumDistance;
//minDistance.Distance = minimumDistance;
//maxDistance = new PointPointDistance((RigidBody)body1, (RigidBody)body2, ((RigidBody)body1).position, ((RigidBody)body2).position);
//maxDistance.Behavior = PointPointDistance.DistanceBehavior.LimitMaximumDistance;
//maxDistance.Distance = maximumDistance;
//StateTracker.AddTracking(fixedAngle);
StateTracker.AddTracking(pointOnLine);
//StateTracker.AddTracking(minDistance);
//StateTracker.AddTracking(maxDistance);
Activate();
}
public LimitHingeJoint3D(IWorld world, IBody3D body1, IBody3D body2, TSVector position, TSVector hingeAxis, FP minLimit, FP maxLimit) : base(world, body1, body2, position, hingeAxis)
{
TSVector perpDir = TSVector.up;
if (TSVector.Dot(perpDir, hingeAxis) > 0.1f)
{
perpDir = TSVector.right;
}
TSVector sideAxis = TSVector.Cross(hingeAxis, perpDir);
perpDir = TSVector.Cross(sideAxis, hingeAxis);
perpDir.Normalize();
FP len = 15;
TSVector hingeRelAnchorPos0 = perpDir * len;
FP angleToMiddle = FP.Half * (minLimit - maxLimit);
TSMatrix outMatrix;
TSMatrix.CreateFromAxisAngle(ref hingeAxis, -angleToMiddle * FP.Deg2Rad, out outMatrix);
TSVector hingeRelAnchorPos1 = TSVector.Transform(hingeRelAnchorPos0, outMatrix);
FP hingeHalfAngle = FP.Half * (minLimit + maxLimit);
FP allowedDistance = len * 2 * FP.Sin(hingeHalfAngle * FP.Half * FP.Deg2Rad);
TSVector hingePos = body1.TSPosition;
TSVector relPos0c = hingePos + hingeRelAnchorPos0;
TSVector relPos1c = hingePos + hingeRelAnchorPos1;
distance = new PointPointDistance((RigidBody)body1, (RigidBody)body2, relPos0c, relPos1c);
distance.Distance = allowedDistance;
distance.Behavior = PointPointDistance.DistanceBehavior.LimitMaximumDistance;
StateTracker.AddTracking(distance);
}
public override void OnSyncedStart()
{
thisBody = GetComponent <TSRigidBody> ();
IBody3D body1 = GetComponent <TSCollider> ().Body;
IBody3D body2 = connectedBody.Body;
Vector3 worldPos = transform.TransformPoint(anchor);
TSVector TSworldPos = worldPos.ToTSVector();
Vector3 worldAxis = transform.TransformDirection(Axis);
TSWorldAxis = worldAxis.ToTSVector();
Debug.Log("Hinge joint");
//if (useLimits)
// thisJoint = new LimitedHingeJoint(PhysicsWorldManager.instance.GetWorld(), body1, body2, TSworldPos, TSWorldAxis, -Limits.Min, Limits.Max);
//else
thisJoint = new HingeJoint3D(PhysicsWorldManager.instance.GetWorld(), body1, body2, TSworldPos, TSWorldAxis);
//charac joint = new CharacterJoint ();
}
/// <summary>
/// Initializes a new instance of the HingeJoint class.
/// </summary>
/// <param name="world">The world class where the constraints get added to.</param>
/// <param name="body1">The first body connected to the second one.</param>
/// <param name="body2">The second body connected to the first one.</param>
/// <param name="position">The position in world space where both bodies get connected.</param>
/// <param name="hingeAxis">The axis if the hinge.</param>
///
public static void Create(IWorld world, IBody3D body1, IBody3D body2, TSVector position, TSVector hingeAxis)
{
new HingeJoint3D(world, body1, body2, position, hingeAxis);
}
/// <summary>
/// Initializes a new instance of the HingeJoint class.
/// </summary>
/// <param name="world">The world class where the constraints get added to.</param>
/// <param name="body1">The first body connected to the second one.</param>
/// <param name="body2">The second body connected to the first one.</param>
/// <param name="position">The position in world space where both bodies get connected.</param>
/// <param name="hingeAxis">The axis if the hinge.</param>
///
public static void Create(IWorld world, IBody3D body1, IBody3D body2, TSVector anchorPosition, TSVector anchorPosition2)
{
new BasicJoint3D(world, body1, body2, anchorPosition, anchorPosition2);
}
public static void TSApplyRelativeTorque(this IBody3D thisBody, TSVector torque)
{
TSVector relCord = TSVector.Transform(torque, thisBody.TSOrientation);
thisBody.TSApplyTorque(relCord);
}
