public override void OnInspectorGUI()
{
B6DOFConstraint hc = (B6DOFConstraint)target;
EditorGUILayout.HelpBox(B6DOFConstraint.HelpMessage, MessageType.Info);
BTypedConstraintEditor.DrawTypedConstraint(hc);
EditorGUILayout.Separator();
EditorGUILayout.LabelField("Limits", EditorStyles.boldLabel);
hc.linearLimitLower = EditorGUILayout.Vector3Field("Linear Limit Lower", hc.linearLimitLower);
hc.linearLimitUpper = EditorGUILayout.Vector3Field("Linear Limit Upper", hc.linearLimitUpper);
hc.angularLimitLowerRadians = EditorGUILayout.Vector3Field("Angular Limit Lower (Deg.)", hc.angularLimitLowerRadians * Mathf.Rad2Deg) * Mathf.Deg2Rad;
hc.angularLimitUpperRadians = EditorGUILayout.Vector3Field("Angular Limit Upper (Deg.)", hc.angularLimitUpperRadians * Mathf.Rad2Deg) * Mathf.Deg2Rad;
EditorGUILayout.Separator();
EditorGUILayout.LabelField("Motor", EditorStyles.boldLabel);
hc.motorLinearTargetVelocity = EditorGUILayout.Vector3Field("Motor Linear Target Velocity", hc.motorLinearTargetVelocity);
hc.motorLinearMaxMotorForce = EditorGUILayout.Vector3Field("Motor Linear Max Force", hc.motorLinearMaxMotorForce);
if (GUI.changed)
{
serializedObject.ApplyModifiedProperties();
EditorUtility.SetDirty(hc);
EditorSceneManager.MarkSceneDirty(EditorSceneManager.GetActiveScene());
Repaint();
}
}
public static void SetSolenoid(RigidNode node, bool forward)
{
float acceleration = 0;
B6DOFConstraint b6DOFConstraint = node.GetJoint <B6DOFConstraint>();
if (b6DOFConstraint == null)
{
return;
}
// TODO: This code is untested - test it.
if (b6DOFConstraint.motorLinearMaxMotorForce.x > 0)
{
acceleration = b6DOFConstraint.motorLinearMaxMotorForce.x / b6DOFConstraint.thisRigidBody.mass * (forward ? 1 : -1);
}
else
{
// TODO: Wth are all these arbitrary numbers??? Make constants.
float psiToNMm2 = 0.00689475728f;
float maximumForce = (psiToNMm2 * 60f) * (Mathf.PI * Mathf.Pow(6.35f, 2f));
acceleration = (maximumForce / b6DOFConstraint.thisRigidBody.mass) * (forward ? 1 : -1);
return;
}
// This is sketchy as heck, could be the cause of any issues that might occur.
float velocity = acceleration * (Time.deltaTime) - Vector3.Dot(b6DOFConstraint.thisRigidBody.velocity,
((RigidBody)node.MainObject.GetComponent <BRigidBody>().GetCollisionObject()).WorldTransform.ToUnity().MultiplyVector(b6DOFConstraint.localConstraintAxisX));
b6DOFConstraint.motorLinearTargetVelocity = new Vector3(velocity, 0f, 0f);
}
/// <summary>
/// Creates node_0 of a manipulator for QuickSwap mode. Node_0 is used to attach the manipulator to the robot.
/// </summary>
public void CreateManipulatorJoint()
{
//Ignore physics/collisions between the manipulator and the robot. Currently not working.
foreach (BRigidBody rb in GameObject.Find("Robot").GetComponentsInChildren <BRigidBody>())
{
MainObject.GetComponent <BRigidBody>().GetCollisionObject().SetIgnoreCollisionCheck(rb.GetCollisionObject(), true);
}
if (joint != null || GetSkeletalJoint() == null)
{
RotationalJoint_Base rNode = new RotationalJoint_Base();
B6DOFConstraint hc = MainObject.AddComponent <B6DOFConstraint>();
hc.thisRigidBody = MainObject.GetComponent <BRigidBody>();
hc.otherRigidBody = GameObject.Find("Robot").GetComponentInChildren <BRigidBody>();
hc.localConstraintPoint = ComOffset;
//Put this after everything else
hc.constraintType = BTypedConstraint.ConstraintType.constrainToAnotherBody;
}
}
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;
}
}