/// <summary>
/// Used for writing the data of a CylindricalJoint_Base.
/// </summary>
/// <param name="joint"></param>
/// <param name="writer"></param>
private static void WriteCylindricalJoint(CylindricalJoint_Base joint, XmlWriter writer)
{
writer.WriteStartElement("CylindricalJoint");
joint.EnforceOrder();
WriteBXDVector3(joint.basePoint, writer, "BasePoint");
WriteBXDVector3(joint.axis, writer, "Axis");
if (joint.hasAngularLimit)
{
writer.WriteElementString("AngularLowLimit", joint.angularLimitLow.ToString("F4"));
writer.WriteElementString("AngularHighLimit", joint.angularLimitHigh.ToString("F4"));
}
if (joint.hasLinearStartLimit)
{
writer.WriteElementString("LinearStartLimit", joint.linearLimitStart.ToString("F4"));
}
if (joint.hasLinearEndLimit)
{
writer.WriteElementString("LinearEndLimit", joint.linearLimitEnd.ToString("F4"));
}
writer.WriteElementString("CurrentLinearPosition", joint.currentLinearPosition.ToString("F4"));
writer.WriteElementString("CurrentAngularPosition", joint.currentAngularPosition.ToString("F4"));
writer.WriteEndElement();
}
/// <summary>
/// Used for writing the data of a CylindricalJoint_Base.
/// </summary>
/// <param name="joint"></param>
/// <param name="writer"></param>
private static void WriteCylindricalJoint(CylindricalJoint_Base joint, XmlWriter writer)
{
writer.WriteStartElement("CylindricalJoint");
joint.EnforceOrder();
WriteBXDVector3(joint.basePoint, writer, "BasePoint");
WriteBXDVector3(joint.axis, writer, "Axis");
if (joint.hasAngularLimit)
{
writer.WriteElementString("AngularLowLimit", joint.angularLimitLow.ToString("F4"));
writer.WriteElementString("AngularHighLimit", joint.angularLimitHigh.ToString("F4"));
}
if (joint.hasLinearStartLimit)
{
writer.WriteElementString("LinearStartLimit", joint.linearLimitStart.ToString("F4"));
}
if (joint.hasLinearEndLimit)
{
writer.WriteElementString("LinearEndLimit", joint.linearLimitEnd.ToString("F4"));
}
//writer.WriteElementString("CurrentLinearPosition", joint.currentLinearPosition.ToString("F4"));
//writer.WriteElementString("CurrentAngularPosition", joint.currentAngularPosition.ToString("F4"));
writer.WriteElementString("CurrentLinearPosition", joint.currentLinearPosition.ToString("F4")); // writes the lowest point of the joint to the file as the positon so the joint starts at the bottom, hopefully prevents any weird issues with the joint limits being messed up do to being relative to the start of the joint
writer.WriteElementString("CurrentAngularPosition", joint.currentAngularPosition.ToString("F4")); // writes the lowest point of the joint to the file as the positon so the joint starts at the bottom, hopefully prevents any weird issues with the joint limits being messed up do to being relative to the start of the joint
writer.WriteEndElement();
}
/// <summary>
/// Reads a CylindricalJoint_Base from the given XmlReader.
/// </summary>
/// <param name="reader"></param>
/// <returns></returns>
private static CylindricalJoint_Base ReadCylindricalJoint_3_0(XmlReader reader)
{
// Create a new CylindricalJoint_Base.
CylindricalJoint_Base cylindricalJoint = (CylindricalJoint_Base)SkeletalJoint_Base.JOINT_FACTORY(SkeletalJointType.CYLINDRICAL);
foreach (string name in IOUtilities.AllElements(reader))
{
switch (name)
{
case "BXDVector3":
switch (reader["VectorID"])
{
case "BasePoint":
// Assign the BXDVector3 to the basePoint.
cylindricalJoint.basePoint = ReadBXDVector3_3_0(reader.ReadSubtree());
break;
case "Axis":
// Assign the BXDVector3 to the axis.
cylindricalJoint.axis = ReadBXDVector3_3_0(reader.ReadSubtree());
break;
}
break;
case "AngularLowLimit":
// Assign a value to the angularLimitLow.
cylindricalJoint.hasAngularLimit = true;
cylindricalJoint.angularLimitLow = float.Parse(reader.ReadElementContentAsString());
break;
case "AngularHighLimit":
// Assign a value to the angularLimitHigh.
cylindricalJoint.angularLimitHigh = float.Parse(reader.ReadElementContentAsString());
break;
case "LinearStartLimit":
// Assign a value to the linearLimitStart.
cylindricalJoint.hasLinearStartLimit = true;
cylindricalJoint.linearLimitStart = float.Parse(reader.ReadElementContentAsString());
break;
case "LinearEndLimit":
// Assign a value to the linearLimitEnd.
cylindricalJoint.hasLinearEndLimit = true;
cylindricalJoint.linearLimitEnd = float.Parse(reader.ReadElementContentAsString());
break;
case "CurrentLinearPosition":
// Assign a value to the currentLinearPosition.
cylindricalJoint.currentLinearPosition = float.Parse(reader.ReadElementContentAsString());
break;
case "CurrentAngularPosition":
// Assign a value to the currentAngularPosition.
cylindricalJoint.currentAngularPosition = float.Parse(reader.ReadElementContentAsString());
break;
}
}
return(cylindricalJoint);
}
/// <summary>
/// Compute the positions and rotations of this node and its children
/// </summary>
/// <param name="moveJoints">Whether or not to move the node on its joints</param>
public void compute(bool moveJoints)
{
if (moveJoints)
{
timeStep += 0.005f;
}
else
{
timeStep = 0.0f;
}
#region INIT_POSITION
if (!initialPositions && GetSkeletalJoint() != null)
{
initialPositions = true;
switch (GetSkeletalJoint().GetJointType())
{
case SkeletalJointType.CYLINDRICAL:
CylindricalJoint_Base cjb = (CylindricalJoint_Base)GetSkeletalJoint();
requestedRotation = cjb.currentAngularPosition;
requestedTranslation = cjb.currentLinearPosition;
break;
case SkeletalJointType.ROTATIONAL:
RotationalJoint_Base rjb = (RotationalJoint_Base)GetSkeletalJoint();
requestedRotation = rjb.currentAngularPosition;
requestedTranslation = 0;
break;
case SkeletalJointType.LINEAR:
LinearJoint_Base ljb = (LinearJoint_Base)GetSkeletalJoint();
requestedRotation = 0;
requestedTranslation = ljb.currentLinearPosition;
break;
}
}
#endregion
myTrans = Matrix4.Identity;
if (GetSkeletalJoint() != null)
{
foreach (AngularDOF dof in GetSkeletalJoint().GetAngularDOF())
{
BXDVector3 axis = dof.rotationAxis;
BXDVector3 basePoint = dof.basePoint;
if (GetParent() != null)
{
basePoint = ((OGL_RigidNode)GetParent()).myTrans.Multiply(basePoint);
axis = ((OGL_RigidNode)GetParent()).myTrans.Rotate(axis);
}
requestedRotation = (float)(Math.Sin(timeStep) + 0.9f) * 1.2f *
(dof.hasAngularLimits() ?
(dof.upperLimit - dof.lowerLimit) / 2.0f : 3.14f) +
(dof.hasAngularLimits() ?
dof.lowerLimit : 0);
requestedRotation = Math.Max(dof.lowerLimit, Math.Min(dof.upperLimit, requestedRotation));
myTrans *= Matrix4.CreateTranslation(-dof.basePoint.ToTK());
myTrans *= Matrix4.CreateFromAxisAngle(dof.rotationAxis.ToTK(), requestedRotation - dof.currentPosition);
myTrans *= Matrix4.CreateTranslation(dof.basePoint.ToTK());
}
foreach (LinearDOF dof in GetSkeletalJoint().GetLinearDOF())
{
requestedTranslation = (float)(Math.Cos(timeStep) + 0.9f) * 1.2f *
(dof.hasLowerLinearLimit() && dof.hasUpperLinearLimit() ?
(dof.upperLimit - dof.lowerLimit) / 2.0f : 3.14f) +
(dof.hasLowerLinearLimit() ?
dof.lowerLimit : 0);
requestedTranslation = Math.Max(dof.lowerLimit, Math.Min(dof.upperLimit, requestedTranslation));
myTrans *= Matrix4.CreateTranslation(dof.translationalAxis.ToTK() * (requestedTranslation - dof.currentPosition));
}
}
if (GetParent() != null)
{
myTrans = myTrans * ((OGL_RigidNode)GetParent()).myTrans;
}
foreach (KeyValuePair <SkeletalJoint_Base, RigidNode_Base> pair in Children)
{
OGL_RigidNode child = ((OGL_RigidNode)pair.Value);
child.compute(moveJoints);
}
}
/// <summary>
/// Crenates the proper joint type for this node.
/// </summary>
public void CreateJoint()
{
if (joint != null || GetSkeletalJoint() == null)
{
return;
}
//this is the conditional for Identifying wheels
if (GetSkeletalJoint().GetJointType() == SkeletalJointType.ROTATIONAL)
{
if (this.HasDriverMeta <WheelDriverMeta>())
{
OrientWheelNormals();
}
RotationalJoint_Base nodeR = (RotationalJoint_Base)GetSkeletalJoint();
//takes the x, y, and z axis information from a custom vector class to unity's vector class
joint = ConfigJointInternal <HingeJoint>(nodeR.basePoint.AsV3(), nodeR.axis.AsV3(), delegate(HingeJoint jointSub)
{
jointSub.useLimits = nodeR.hasAngularLimit;
if (nodeR.hasAngularLimit)
{
AngularLimit(jointSub,
MathfExt.ToDegrees(nodeR.currentAngularPosition),
MathfExt.ToDegrees(nodeR.angularLimitLow),
MathfExt.ToDegrees(nodeR.angularLimitHigh));
}
});
//don't worry, I'm a doctor
if (this.HasDriverMeta <WheelDriverMeta>())
{
CreateWheel();
}
}
else if (GetSkeletalJoint().GetJointType() == SkeletalJointType.CYLINDRICAL)
{
CylindricalJoint_Base nodeC = (CylindricalJoint_Base)GetSkeletalJoint();
joint = ConfigJointInternal <ConfigurableJoint>(nodeC.basePoint.AsV3(), nodeC.axis.AsV3(), delegate(ConfigurableJoint jointSub)
{
jointSub.xMotion = ConfigurableJointMotion.Limited;
jointSub.angularXMotion = !nodeC.hasAngularLimit ? ConfigurableJointMotion.Free : ConfigurableJointMotion.Limited;
LinearLimit(jointSub,
nodeC.currentLinearPosition,
nodeC.linearLimitStart,
nodeC.linearLimitEnd);
if (GetSkeletalJoint().cDriver != null && GetSkeletalJoint().cDriver.GetDriveType().IsPneumatic())
{
JointDrive drMode = new JointDrive();
drMode.mode = JointDriveMode.Velocity;
drMode.maximumForce = 100.0f;
jointSub.xDrive = drMode;
}
if (jointSub.angularXMotion == ConfigurableJointMotion.Limited)
{
AngularLimit(jointSub,
MathfExt.ToDegrees(nodeC.currentAngularPosition),
MathfExt.ToDegrees(nodeC.angularLimitLow),
MathfExt.ToDegrees(nodeC.angularLimitHigh));
}
});
}
else if (GetSkeletalJoint().GetJointType() == SkeletalJointType.LINEAR)
{
LinearJoint_Base nodeL = (LinearJoint_Base)GetSkeletalJoint();
joint = ConfigJointInternal <ConfigurableJoint>(nodeL.basePoint.AsV3(), nodeL.axis.AsV3(), delegate(ConfigurableJoint jointSub)
{
jointSub.xMotion = ConfigurableJointMotion.Limited;
LinearLimit(jointSub,
nodeL.currentLinearPosition,
nodeL.linearLimitLow,
nodeL.linearLimitHigh);
});
//TODO make code good
if (GetSkeletalJoint().cDriver != null)
{
if (GetSkeletalJoint().cDriver.GetDriveType().IsElevator())
{
Debug.Log(GetSkeletalJoint().cDriver.portA);
unityObject.AddComponent <ElevatorScript>();
unityObject.GetComponent <ElevatorScript>().eType = (ElevatorType)this.GetDriverMeta <ElevatorDriverMeta>().type;
Debug.Log("added");
}
}
}
SetXDrives();
}
public void CreateJoint(RobotBase robotBase, float wheelFriction = 1f, float lateralFriction = 1f)
{
if (joint != null || GetSkeletalJoint() == null)
{
return;
}
switch (GetSkeletalJoint().GetJointType())
{
case SkeletalJointType.ROTATIONAL:
if (this.HasDriverMeta <WheelDriverMeta>() && this.GetDriverMeta <WheelDriverMeta>().type != WheelType.NOT_A_WHEEL)
{
RigidNode parent = (RigidNode)GetParent();
if (parent.MainObject.GetComponent <BRaycastRobot>() == null)
{
parent.MainObject.AddComponent <BRaycastRobot>().RootNode = RootNode;
}
WheelType wheelType = this.GetDriverMeta <WheelDriverMeta>().type;
BRaycastWheel wheel = MainObject.AddComponent <BRaycastWheel>();
wheel.CreateWheel(this);
if (robotBase is MaMRobot)
{
wheel.Friction = wheelFriction;
wheel.SlidingFriction = lateralFriction;
}
MainObject.transform.parent = parent.MainObject.transform;
}
else
{
RotationalJoint_Base rNode = (RotationalJoint_Base)GetSkeletalJoint();
BHingedConstraintEx hc = (BHingedConstraintEx)(joint = ConfigJoint <BHingedConstraintEx>(rNode.basePoint.AsV3() - ComOffset, rNode.axis.AsV3(), AxisType.X));
Vector3 rAxis = rNode.axis.AsV3().normalized;
hc.axisInA = rAxis;
hc.axisInB = rAxis;
if (hc.setLimit = rNode.hasAngularLimit)
{
hc.lowLimitAngleRadians = rNode.currentAngularPosition - rNode.angularLimitHigh;
hc.highLimitAngleRadians = rNode.currentAngularPosition - rNode.angularLimitLow;
}
hc.constraintType = BTypedConstraint.ConstraintType.constrainToAnotherBody;
}
break;
case SkeletalJointType.CYLINDRICAL:
CylindricalJoint_Base cNode = (CylindricalJoint_Base)GetSkeletalJoint();
Vector3 cAxis = cNode.axis.AsV3().normalized;
B6DOFConstraint bc = (B6DOFConstraint)(joint = ConfigJoint <B6DOFConstraint>(cNode.basePoint.AsV3() - ComOffset, cAxis, AxisType.X));
bc.localConstraintAxisX = cAxis;
bc.localConstraintAxisY = new Vector3(cAxis.y, cAxis.z, cAxis.x);
bc.linearLimitLower = new Vector3((cNode.linearLimitStart - cNode.currentLinearPosition) * 0.01f, 0f, 0f);
bc.linearLimitUpper = new Vector3((cNode.linearLimitEnd - cNode.currentLinearPosition) * 0.01f, 0f, 0f);
// TODO: Implement angular cylinder limits
bc.constraintType = BTypedConstraint.ConstraintType.constrainToAnotherBody;
break;
case SkeletalJointType.LINEAR:
LinearJoint_Base lNode = (LinearJoint_Base)GetSkeletalJoint();
Vector3 lAxis = lNode.axis.AsV3().normalized;
BSliderConstraint sc = (BSliderConstraint)(joint = ConfigJoint <BSliderConstraint>(lNode.basePoint.AsV3() - ComOffset, lAxis, AxisType.X));
sc.localConstraintAxisX = lAxis;
sc.localConstraintAxisY = new Vector3(lAxis.y, lAxis.z, lAxis.x);
sc.lowerLinearLimit = (lNode.linearLimitLow - lNode.currentLinearPosition) * 0.01f;
sc.upperLinearLimit = (lNode.linearLimitHigh - lNode.currentLinearPosition) * 0.01f;
sc.lowerAngularLimitRadians = 0f;
sc.upperAngularLimitRadians = 0f;
sc.constraintType = BTypedConstraint.ConstraintType.constrainToAnotherBody;
bool b = this.HasDriverMeta <ElevatorDriverMeta>();
if (GetSkeletalJoint().cDriver != null)
{
if (GetSkeletalJoint().cDriver.GetDriveType().IsElevator())
{
MainObject.GetComponent <BRigidBody>().mass *= 2f;
}
}
break;
}
}
public void CreateJoint()
{
if (joint != null || GetSkeletalJoint() == null)
{
return;
}
switch (GetSkeletalJoint().GetJointType())
{
case SkeletalJointType.ROTATIONAL:
WheelType wheelType = WheelType.NOT_A_WHEEL;
if (this.HasDriverMeta <WheelDriverMeta>())
{
OrientWheelNormals();
wheelType = this.GetDriverMeta <WheelDriverMeta>().type;
}
RotationalJoint_Base rNode = (RotationalJoint_Base)GetSkeletalJoint();
BHingedConstraintEx hc = (BHingedConstraintEx)(joint = ConfigJoint <BHingedConstraintEx>(rNode.basePoint.AsV3() - ComOffset, rNode.axis.AsV3(), AxisType.X));
Vector3 rAxis = rNode.axis.AsV3().normalized;
hc.axisInA = rAxis;
hc.axisInB = rAxis;
if (hc.setLimit = rNode.hasAngularLimit)
{
hc.lowLimitAngleRadians = rNode.currentAngularPosition - rNode.angularLimitHigh;
hc.highLimitAngleRadians = rNode.currentAngularPosition - rNode.angularLimitLow;
}
hc.constraintType = BTypedConstraint.ConstraintType.constrainToAnotherBody;
break;
case SkeletalJointType.CYLINDRICAL:
CylindricalJoint_Base cNode = (CylindricalJoint_Base)GetSkeletalJoint();
B6DOFConstraint bc = (B6DOFConstraint)(joint = ConfigJoint <B6DOFConstraint>(cNode.basePoint.AsV3() - ComOffset, cNode.axis.AsV3(), AxisType.X));
bc.linearLimitLower = new Vector3(cNode.linearLimitStart * 0.01f, 0f, 0f);
bc.linearLimitUpper = new Vector3(cNode.linearLimitEnd * 0.01f, 0f, 0f);
bc.constraintType = BTypedConstraint.ConstraintType.constrainToAnotherBody;
break;
case SkeletalJointType.LINEAR:
LinearJoint_Base lNode = (LinearJoint_Base)GetSkeletalJoint();
Vector3 lAxis = lNode.axis.AsV3().normalized;
// TODO: Figure out how to make a vertical slider?
BSliderConstraint sc = (BSliderConstraint)(joint = ConfigJoint <BSliderConstraint>(lNode.basePoint.AsV3() - ComOffset, lNode.axis.AsV3(), AxisType.X));
if (lAxis.x < 0)
{
lAxis.x *= -1f;
}
if (lAxis.y < 0)
{
lAxis.y *= -1f;
}
if (lAxis.z < 0)
{
lAxis.z *= -1f;
}
sc.localConstraintAxisX = lAxis;
sc.localConstraintAxisY = new Vector3(lAxis.y, lAxis.z, lAxis.x);
sc.lowerLinearLimit = lNode.linearLimitLow * 0.01f;
sc.upperLinearLimit = lNode.linearLimitHigh * 0.01f;
sc.lowerAngularLimitRadians = 0f;
sc.upperAngularLimitRadians = 0f;
sc.constraintType = BTypedConstraint.ConstraintType.constrainToAnotherBody;
bool b = this.HasDriverMeta <ElevatorDriverMeta>();
if (GetSkeletalJoint().cDriver != null)
{
if (GetSkeletalJoint().cDriver.GetDriveType().IsElevator())
{
MainObject.GetComponent <BRigidBody>().mass *= 2f;
}
}
break;
}
}
public void CreateJoint(int numWheels, bool mixAndMatch, float wheelFriction = 1f, float lateralFriction = 1f)
{
if (joint != null || GetSkeletalJoint() == null)
{
return;
}
switch (GetSkeletalJoint().GetJointType())
{
case SkeletalJointType.ROTATIONAL:
if (this.HasDriverMeta <WheelDriverMeta>() && this.GetDriverMeta <WheelDriverMeta>().type != WheelType.NOT_A_WHEEL)
{
RigidNode parent = (RigidNode)GetParent();
if (parent.MainObject.GetComponent <BRaycastRobot>() == null)
{
BRaycastRobot robot = parent.MainObject.AddComponent <BRaycastRobot>();
robot.NumWheels = numWheels;
}
WheelType wheelType = this.GetDriverMeta <WheelDriverMeta>().type;
BRaycastWheel wheel = MainObject.AddComponent <BRaycastWheel>();
wheel.CreateWheel(this);
if (mixAndMatch)
{
wheel.Friction = wheelFriction;
wheel.SlidingFriction = lateralFriction;
}
MainObject.transform.parent = parent.MainObject.transform;
}
else
{
RotationalJoint_Base rNode = (RotationalJoint_Base)GetSkeletalJoint();
rNode.basePoint.x *= -1;
BHingedConstraintEx hc = (BHingedConstraintEx)(joint = ConfigJoint <BHingedConstraintEx>(rNode.basePoint.AsV3() - ComOffset, rNode.axis.AsV3(), AxisType.X));
Vector3 rAxis = rNode.axis.AsV3().normalized;
rAxis.x *= -1f;
hc.axisInA = rAxis;
hc.axisInB = rAxis;
if (hc.setLimit = rNode.hasAngularLimit)
{
hc.lowLimitAngleRadians = rNode.currentAngularPosition - rNode.angularLimitHigh;
hc.highLimitAngleRadians = rNode.currentAngularPosition - rNode.angularLimitLow;
}
hc.constraintType = BTypedConstraint.ConstraintType.constrainToAnotherBody;
}
break;
case SkeletalJointType.CYLINDRICAL:
CylindricalJoint_Base cNode = (CylindricalJoint_Base)GetSkeletalJoint();
B6DOFConstraint bc = (B6DOFConstraint)(joint = ConfigJoint <B6DOFConstraint>(cNode.basePoint.AsV3() - ComOffset, cNode.axis.AsV3(), AxisType.X));
bc.linearLimitLower = new Vector3(cNode.linearLimitStart * 0.01f, 0f, 0f);
bc.linearLimitUpper = new Vector3(cNode.linearLimitEnd * 0.01f, 0f, 0f);
bc.constraintType = BTypedConstraint.ConstraintType.constrainToAnotherBody;
break;
case SkeletalJointType.LINEAR:
LinearJoint_Base lNode = (LinearJoint_Base)GetSkeletalJoint();
lNode.basePoint.x *= -1;
Vector3 lAxis = lNode.axis.AsV3().normalized;
// TODO: Figure out how to make a vertical slider?
BSliderConstraint sc = (BSliderConstraint)(joint = ConfigJoint <BSliderConstraint>(lNode.basePoint.AsV3() - ComOffset, lNode.axis.AsV3(), AxisType.X));
if (lAxis.x < 0)
{
lAxis.x *= -1f;
}
if (lAxis.y < 0)
{
lAxis.y *= -1f;
}
if (lAxis.z < 0)
{
lAxis.z *= -1f;
}
sc.localConstraintAxisX = lAxis;
sc.localConstraintAxisY = new Vector3(lAxis.y, lAxis.z, lAxis.x);
sc.lowerLinearLimit = lNode.linearLimitLow * 0.01f;
sc.upperLinearLimit = lNode.linearLimitHigh * 0.01f;
sc.lowerAngularLimitRadians = 0f;
sc.upperAngularLimitRadians = 0f;
sc.constraintType = BTypedConstraint.ConstraintType.constrainToAnotherBody;
bool b = this.HasDriverMeta <ElevatorDriverMeta>();
if (GetSkeletalJoint().cDriver != null)
{
if (GetSkeletalJoint().cDriver.GetDriveType().IsElevator())
{
MainObject.GetComponent <BRigidBody>().mass *= 2f;
}
}
break;
}
}
public AngularDOF_Impl(CylindricalJoint_Base cjb)
{
this.cjb = cjb;
}
public LinearDOF_Impl(CylindricalJoint_Base cjb)
{
this.cjb = cjb;
}
