/// <summary>
/// Return the robot to robotStartPosition and destroy extra game pieces
/// </summary>
/// <param name="resetTransform"></param>
public void BeginReset()
{
IsResetting = true;
foreach (RigidNode n in rootNode.ListAllNodes())
{
BRigidBody br = n.MainObject.GetComponent <BRigidBody>();
if (br == null)
{
continue;
}
RigidBody r = (RigidBody)br.GetCollisionObject();
r.LinearVelocity = r.AngularVelocity = BulletSharp.Math.Vector3.Zero;
r.LinearFactor = r.AngularFactor = BulletSharp.Math.Vector3.Zero;
BulletSharp.Math.Matrix newTransform = r.WorldTransform;
newTransform.Origin = (robotStartPosition + n.ComOffset).ToBullet();
newTransform.Basis = BulletSharp.Math.Matrix.Identity;
r.WorldTransform = newTransform;
}
int isMixAndMatch = PlayerPrefs.GetInt("mixAndMatch"); // 0 is false, 1 is true
if (robotHasManipulator == 1 && isMixAndMatch == 1)
{
foreach (RigidNode n in manipulatorNode.ListAllNodes())
{
BRigidBody br = n.MainObject.GetComponent <BRigidBody>();
if (br == null)
{
continue;
}
RigidBody r = (RigidBody)br.GetCollisionObject();
r.LinearVelocity = r.AngularVelocity = BulletSharp.Math.Vector3.Zero;
r.LinearFactor = r.AngularFactor = BulletSharp.Math.Vector3.Zero;
BulletSharp.Math.Matrix newTransform = r.WorldTransform;
newTransform.Origin = (robotStartPosition + n.ComOffset).ToBullet();
newTransform.Basis = BulletSharp.Math.Matrix.Identity;
r.WorldTransform = newTransform;
}
}
//Where "save orientation" works
RotateRobot(robotStartOrientation);
GameObject.Find("Robot").transform.GetChild(0).transform.position = new Vector3(10, 20, 5);
if (IsResetting)
{
Debug.Log("is resetting!");
}
}
void BeginReset()
{
foreach (RigidNode n in rootNode.ListAllNodes())
{
RigidBody r = (RigidBody)n.MainObject.GetComponent <BRigidBody>().GetCollisionObject();
r.LinearVelocity = r.AngularVelocity = BulletSharp.Math.Vector3.Zero;
r.LinearFactor = r.AngularFactor = BulletSharp.Math.Vector3.Zero;
BulletSharp.Math.Matrix newTransform = r.WorldTransform;
newTransform.Origin = (robotStartPosition + n.ComOffset).ToBullet();
newTransform.Basis = BulletSharp.Math.Matrix.Identity;
r.WorldTransform = newTransform;
}
}
/// <summary>
/// Drives all motors associated with a PWM port
/// </summary>
/// <param name="skeleton">robot</param>
/// <param name="e">keyboard args</param>
public static void UpdateAllMotors(RigidNode_Base skeleton, KeyboardKeyEventArgs e)
{
float[] pwm = new float[5];
pwm[0] =
e.Key == Controls.GameControls[Controls.Control.Forward] ? 1 : 0 +
e.Key == Controls.GameControls[Controls.Control.Backward] ? -1 : 0 +
e.Key == Controls.GameControls[Controls.Control.Left] ? -1 : 0 +
e.Key == Controls.GameControls[Controls.Control.Right] ? 1 : 0;
pwm[1] =
e.Key == Controls.GameControls[Controls.Control.Forward] ? -1 : 0 +
e.Key == Controls.GameControls[Controls.Control.Backward] ? 1 : 0 +
e.Key == Controls.GameControls[Controls.Control.Left] ? -1 : 0 +
e.Key == Controls.GameControls[Controls.Control.Right] ? 1 : 0;
pwm[2] = pwm[3] = pwm[4] = 0;
List <RigidNode_Base> subNodes = skeleton.ListAllNodes();
for (int i = 0; i < pwm.Length; i++)
{
foreach (RigidNode_Base node in subNodes)
{
BulletRigidNode bNode = (BulletRigidNode)node;
if (bNode?.GetSkeletalJoint()?.cDriver?.GetDriveType().IsMotor() ?? false)
{
if (bNode.GetSkeletalJoint().cDriver.portA == i + 1)
{
bNode.Update?.Invoke(pwm[i] * 10);
}
}
}
}
}
public void UpdateComponents(RigidNode_Base skeletonBase)
{
ExportedNode = skeletonBase;
List<RigidNode_Base> nodes = new List<RigidNode_Base>();
skeletonBase.ListAllNodes(nodes);
nodeEditorPane1.AddNodes(nodes);
}
bool LoadRobot(string directory)
{
robotObject = new GameObject("Robot");
robotObject.transform.position = robotStartPosition;
RigidNode_Base.NODE_FACTORY = delegate(Guid guid)
{
return(new RigidNode(guid));
};
List <RigidNode_Base> nodes = new List <RigidNode_Base>();
rootNode = BXDJSkeleton.ReadSkeleton(directory + "\\skeleton.bxdj");
rootNode.ListAllNodes(nodes);
foreach (RigidNode_Base n in nodes)
{
RigidNode node = (RigidNode)n;
node.CreateTransform(robotObject.transform);
if (!node.CreateMesh(directory + "\\" + node.ModelFileName))
{
Debug.Log("Robot not loaded!");
Destroy(robotObject);
return(false);
}
node.CreateJoint();
}
return(true);
}
private void openToolStripMenuItem_Click(object sender, EventArgs e)
{
robotViewer1 = new RobotViewer();
FolderBrowserDialog browser = new FolderBrowserDialog();
if (browser.ShowDialog() == DialogResult.OK && browser.SelectedPath != null)
{
RigidNode_Base node = BXDJSkeleton.ReadSkeleton(browser.SelectedPath + @"\skeleton.bxdj");
List <RigidNode_Base> nodes = node.ListAllNodes();
List <BXDAMesh> meshes = new List <BXDAMesh>();
foreach (RigidNode_Base n in nodes)
{
BXDAMesh mesh = new BXDAMesh();
mesh.ReadFromFile(browser.SelectedPath + "\\" + n.ModelFileName);
if (!n.GUID.Equals(mesh.GUID))
{
MessageBox.Show(n.ModelFileName + " has been modified.", "Could not load mesh.");
}
meshes.Add(mesh);
}
robotViewer1.LoadModel(node, meshes);
robotViewer1.FixLimits();
robotViewer1.HighlightAll();
Text = "Robot Viewer: " + browser.SelectedPath.Split(new char[] { '\\' }, System.StringSplitOptions.RemoveEmptyEntries)[browser.SelectedPath.Split(new char[] { '\\' }, System.StringSplitOptions.RemoveEmptyEntries).Length - 1];
}
}
/// <summary>
/// Gets an array of all the names at and below a node.
/// </summary>
/// <param name="baseNode"></param>
/// <returns></returns>
public static string[] GetExportedComponentNames(RigidNode_Base baseNode)
{
List <string> names = new List <string>();
foreach (RigidNode_Base node in baseNode.ListAllNodes())
{
names.AddRange(node.ModelFullID.Split(new string[] { "-_-" }, StringSplitOptions.RemoveEmptyEntries));
}
return(names.ToArray());
}
public static List <BXDAMesh> ExportMeshes(RigidNode_Base baseNode, bool useOCL = false)
{
SurfaceExporter surfs = new SurfaceExporter();
BXDJSkeleton.SetupFileNames(baseNode, true);
List <RigidNode_Base> nodes = new List <RigidNode_Base>();
baseNode.ListAllNodes(nodes);
SynthesisGUI.Instance.ExporterSetMeshes(nodes.Count);
List <BXDAMesh> meshes = new List <BXDAMesh>();
foreach (RigidNode_Base node in nodes)
{
SynthesisGUI.Instance.ExporterSetOverallText("Exporting " + node.ModelFileName);
if (node is RigidNode && node.GetModel() != null && node.ModelFileName != null && node.GetModel() is CustomRigidGroup)
{
Console.WriteLine("Exporting " + node.ModelFileName);
try
{
SynthesisGUI.Instance.ExporterReset();
CustomRigidGroup group = (CustomRigidGroup)node.GetModel();
surfs.Reset(node.GUID);
Console.WriteLine("Exporting meshes...");
surfs.ExportAll(group, (long progress, long total) =>
{
double totalProgress = (((double)progress / (double)total) * 100.0);
SynthesisGUI.Instance.ExporterSetSubText(String.Format("Export {1} / {2}", Math.Round(totalProgress, 2), progress, total));
SynthesisGUI.Instance.ExporterSetProgress(totalProgress);
});
Console.WriteLine();
BXDAMesh output = surfs.GetOutput();
Console.WriteLine("Output: " + output.meshes.Count + " meshes");
Console.WriteLine("Computing colliders...");
output.colliders.Clear();
output.colliders.AddRange(ConvexHullCalculator.GetHull(output));
meshes.Add(output);
}
catch (Exception e)
{
Console.WriteLine(e.ToString());
throw new Exception("Error exporting mesh: " + node.GetModelID());
}
}
SynthesisGUI.Instance.ExporterStepOverall();
}
return(meshes);
}
public bool LoadManipulator(string directory)
{
manipulatorObject = new GameObject("Manipulator");
//Set the manipulator transform to match with the position of node_0 of the robot. THIS ONE ACTUALLY DOES SOMETHING:
manipulatorObject.transform.position = GameObject.Find("Robot").transform.GetChild(0).transform.position;
//manipulatorObject.transform.position = robotStartPosition;
RigidNode_Base.NODE_FACTORY = delegate(Guid guid)
{
return(new RigidNode(guid));
};
List <RigidNode_Base> nodes = new List <RigidNode_Base>();
//TO-DO: Read .robot instead (from the new exporters if they are implemented). Maybe need a RobotSkeleton class
manipulatorNode = BXDJSkeleton.ReadSkeleton(directory + "\\skeleton.bxdj");
manipulatorNode.ListAllNodes(nodes);
//Load node_0 for attaching manipulator to robot
RigidNode node = (RigidNode)nodes[0];
node.CreateTransform(manipulatorObject.transform);
if (!node.CreateMesh(directory + "\\" + node.ModelFileName))
{
Debug.Log("Robot not loaded!");
UnityEngine.Object.Destroy(manipulatorObject);
return(false);
}
node.CreateManipulatorJoint();
node.MainObject.AddComponent <Tracker>().Trace = true;
Tracker t = node.MainObject.GetComponent <Tracker>();
Debug.Log(t);
//Load other nodes associated with the manipulator
for (int i = 1; i < nodes.Count; i++)
{
RigidNode otherNode = (RigidNode)nodes[i];
otherNode.CreateTransform(manipulatorObject.transform);
if (!otherNode.CreateMesh(directory + "\\" + otherNode.ModelFileName))
{
Debug.Log("Robot not loaded!");
UnityEngine.Object.Destroy(manipulatorObject);
return(false);
}
otherNode.CreateJoint();
otherNode.MainObject.AddComponent <Tracker>().Trace = true;
t = otherNode.MainObject.GetComponent <Tracker>();
Debug.Log(t);
}
RotateRobot(robotStartOrientation);
return(true);
}
/// <summary>
/// Initializes physical robot based off of robot directory.
/// </summary>
/// <param name="directory">folder directory of robot</param>
/// <returns></returns>
public bool InitializeRobot(string directory, MainState source)
{
//Deletes all nodes if any exist
int childCount = transform.childCount;
for (int i = 0; i < childCount; ++i)
{
Destroy(transform.GetChild(i).gameObject);
}
mainState = source;
transform.position = robotStartPosition;
RigidNode_Base.NODE_FACTORY = delegate(Guid guid)
{
return(new RigidNode(guid));
};
List <RigidNode_Base> nodes = new List <RigidNode_Base>();
//Read .robot instead. Maybe need a RobotSkeleton class
rootNode = BXDJSkeleton.ReadSkeleton(directory + "\\skeleton.bxdj");
rootNode.ListAllNodes(nodes);
foreach (RigidNode_Base n in nodes)
{
RigidNode node = (RigidNode)n;
node.CreateTransform(transform);
if (!node.CreateMesh(directory + "\\" + node.ModelFileName))
{
Debug.Log("Robot not loaded!");
return(false);
}
node.CreateJoint();
node.MainObject.AddComponent <Tracker>().Trace = true;
}
RotateRobot(robotStartOrientation);
RobotName = new DirectoryInfo(directory).Name;
isInitialized = true;
robotCamera = GameObject.Find("RobotCameraList").GetComponent <RobotCamera>();
//Attached to the main frame and face the front
robotCamera.AddCamera(transform.GetChild(0).transform);
//Attached to the first node and face the front
robotCamera.AddCamera(transform.GetChild(1).transform);
////Attached to main frame and face the back
robotCamera.AddCamera(transform.GetChild(0).transform, new Vector3(0, 0, 0), new Vector3(0, 180, 0));
return(true);
}
// TODO: Something weird is going on with the spawn, at least with robots with manipulators. Reset is fine.
/// <summary>
/// Loads and initializes the physical manipulator object (used in Mix and Match mode)
/// </summary>
/// <param name="directory">Folder directory of the manipulator</param>
/// <param name="robotGameObject">GameObject of the robot the manipulator will be attached to</param>
public bool InitializeManipulator(string directory)
{
ManipulatorObject = new GameObject("Manipulator");
ManipulatorObject.transform.position = robotStartPosition + manipulatorOffset;
RigidNode_Base.NODE_FACTORY = delegate(Guid guid) { return(new RigidNode(guid)); };
List <RigidNode_Base> nodes = new List <RigidNode_Base>();
//TO-DO: Read .robot instead (from the new exporters if they are implemented). Maybe need a RobotSkeleton class
manipulatorNode = BXDJSkeleton.ReadSkeleton(directory + "\\skeleton.bxdj");
manipulatorNode.ListAllNodes(nodes);
//Load node_0 for attaching manipulator to robot
RigidNode node = (RigidNode)nodes[0];
node.CreateTransform(ManipulatorObject.transform);
if (!node.CreateMesh(directory + "\\" + node.ModelFileName))
{
Destroy(ManipulatorObject);
return(false);
}
node.CreateManipulatorJoint(gameObject);
node.MainObject.AddComponent <Tracker>().Trace = true;
//Load other nodes associated with the manipulator
for (int i = 1; i < nodes.Count; i++)
{
RigidNode otherNode = (RigidNode)nodes[i];
otherNode.CreateTransform(ManipulatorObject.transform);
if (!otherNode.CreateMesh(directory + "\\" + otherNode.ModelFileName))
{
Destroy(ManipulatorObject);
return(false);
}
otherNode.MainObject.AddComponent <Tracker>().Trace = true;
}
RootNode.GenerateWheelInfo();
for (int i = 1; i < nodes.Count; i++)
{
RigidNode otherNode = (RigidNode)nodes[i];
otherNode.CreateJoint(this);
}
RotateRobot(robotStartOrientation);
RobotHasManipulator = true;
return(true);
}
/// <summary>
/// Load a list of nodes into the editor pane
/// </summary>
/// <param name="root">The base node</param>
public void SetSkeleton(RigidNode_Base root)
{
if (root == null)
{
nodeList = null;
}
else
{
nodeList = root.ListAllNodes();
}
UpdateJointList();
}
/// <summary>
/// Return the robot to robotStartPosition and destroy extra game pieces
/// </summary>
/// <param name="resetTransform"></param>
public void BeginReset()
{
IsResetting = true;
foreach (Tracker t in UnityEngine.Object.FindObjectsOfType <Tracker>())
{
t.Clear();
}
foreach (RigidNode n in rootNode.ListAllNodes())
{
RigidBody r = (RigidBody)n.MainObject.GetComponent <BRigidBody>().GetCollisionObject();
r.LinearVelocity = r.AngularVelocity = BulletSharp.Math.Vector3.Zero;
r.LinearFactor = r.AngularFactor = BulletSharp.Math.Vector3.Zero;
BulletSharp.Math.Matrix newTransform = r.WorldTransform;
newTransform.Origin = (robotStartPosition + n.ComOffset).ToBullet();
newTransform.Basis = BulletSharp.Math.Matrix.Identity;
r.WorldTransform = newTransform;
}
RotateRobot(robotStartOrientation);
if (IsResetting)
{
Debug.Log("is resetting!");
}
}
/// <summary>
/// Gets an array of all the exported <see cref="ComponentOccurrence"/>s at and below a node.
/// </summary>
/// <param name="baseNode"></param>
/// <returns></returns>
public static ComponentOccurrence[] GetExportedComponents(RigidNode_Base baseNode)
{
List <ComponentOccurrence> names = new List <ComponentOccurrence>();
foreach (RigidNode_Base node in baseNode.ListAllNodes())
{
foreach (string s in node.ModelFullID.Split(new string[] { "-_-" }, StringSplitOptions.RemoveEmptyEntries))
{
names.Add(StandardAddInServer.Instance.GetOccurrence(s));
}
}
return(names.ToArray());
}
/// <summary>
/// Ensures that every node is assigned a model file name by assigning all nodes without a file name a generated name.
/// </summary>
/// <param name="baseNode">The base node of the skeleton</param>
/// <param name="overwrite">Overwrite existing</param>
public static void SetupFileNames(RigidNode_Base baseNode)
{
List <RigidNode_Base> nodes = new List <RigidNode_Base>();
baseNode.ListAllNodes(nodes);
for (int i = 0; i < nodes.Count; i++)
{
if (nodes[i].ModelFileName == null)
{
nodes[i].ModelFileName = ("node_" + i + ".bxda");
}
}
}
/// <summary>
/// Open a previously exported robot.
/// </summary>
/// <param name="validate">If it is not null, this will validate the open inventor assembly.</param>
public void OpenExisting()
{
if (SkeletonBase != null && !WarnUnsaved())
{
return;
}
string dirPath = OpenFolderPath();
if (dirPath == null)
{
return;
}
try
{
List <RigidNode_Base> nodes = new List <RigidNode_Base>();
SkeletonBase = BXDJSkeleton.ReadSkeleton(dirPath + "\\skeleton.bxdj");
SkeletonBase.ListAllNodes(nodes);
Meshes = new List <BXDAMesh>();
foreach (RigidNode_Base n in nodes)
{
BXDAMesh mesh = new BXDAMesh();
mesh.ReadFromFile(dirPath + "\\" + n.ModelFileName);
if (!n.GUID.Equals(mesh.GUID))
{
MessageBox.Show(n.ModelFileName + " has been modified.", "Could not load mesh.");
return;
}
Meshes.Add(mesh);
}
for (int i = 0; i < Meshes.Count; i++)
{
((OGL_RigidNode)nodes[i]).loadMeshes(Meshes[i]);
}
}
catch (Exception e)
{
MessageBox.Show(e.ToString());
}
ReloadPanels();
}
/// <summary>
/// Called when the robot begins to reset.
/// </summary>
protected override void OnBeginReset()
{
if (!RobotHasManipulator)
{
return;
}
int i = 0;
foreach (RigidNode n in manipulatorNode.ListAllNodes())
{
BRigidBody br = n.MainObject.GetComponent <BRigidBody>();
if (br == null)
{
continue;
}
RigidBody r = (RigidBody)br.GetCollisionObject();
r.LinearVelocity = r.AngularVelocity = BulletSharp.Math.Vector3.Zero;
r.LinearFactor = r.AngularFactor = BulletSharp.Math.Vector3.Zero;
BulletSharp.Math.Matrix newTransform = r.WorldTransform;
newTransform.Origin = (robotStartPosition + n.ComOffset).ToBullet();
newTransform.Basis = BulletSharp.Math.Matrix.Identity;
r.WorldTransform = newTransform;
if (i == 0)
{
Debug.Log("Transform Origin" + newTransform.Origin);
}
i++;
}
}
/// <summary>
/// The lite equivalent of the 'Start Exporter' <see cref="Button"/> in the <see cref="ExporterForm"/>. Used in <see cref="ExporterWorker_DoWork(Object, "/>
/// </summary>
/// <seealso cref="ExporterWorker_DoWork"/>
/// <param name="baseNode"></param>
/// <returns></returns>
public List <BXDAMesh> ExportMeshesLite(RigidNode_Base baseNode)
{
SurfaceExporter surfs = new SurfaceExporter();
BXDJSkeleton.SetupFileNames(baseNode, true);
List <RigidNode_Base> nodes = new List <RigidNode_Base>();
baseNode.ListAllNodes(nodes);
List <BXDAMesh> meshes = new List <BXDAMesh>();
foreach (RigidNode_Base node in nodes)
{
SetProgressText("Exporting " + node.ModelFileName);
if (node is RigidNode && node.GetModel() != null && node.ModelFileName != null && node.GetModel() is CustomRigidGroup)
{
try
{
CustomRigidGroup group = (CustomRigidGroup)node.GetModel();
surfs.Reset(node.GUID);
surfs.ExportAll(group, (long progress, long total) =>
{
SetProgressText(String.Format("Export {0} / {1}", progress, total));
});
BXDAMesh output = surfs.GetOutput();
output.colliders.Clear();
output.colliders.AddRange(ConvexHullCalculator.GetHull(output));
meshes.Add(output);
}
catch (Exception e)
{
throw new Exception("Error exporting mesh: " + node.GetModelID(), e);
}
}
}
return(meshes);
}
/// <summary>
/// Checks if a baseNode matches up with the assembly. Passed as a <see cref="ValidationAction"/> to
/// </summary>
/// <param name="baseNode"></param>
/// <param name="message"></param>
/// <returns></returns>
private static bool ValidateAssembly(RigidNode_Base baseNode, out string message)
{
var validationCount = 0;
var failedCount = 0;
var nodes = baseNode.ListAllNodes();
foreach (var node in nodes)
{
var failedValidation = false;
foreach (var componentName in node.ModelFullID.Split(new string[] { "-_-" },
StringSplitOptions.RemoveEmptyEntries))
{
if (!CheckForOccurrence(componentName))
{
failedCount++;
failedValidation = true;
}
}
if (!failedValidation)
{
validationCount++;
}
}
if (validationCount == nodes.Count)
{
message = String.Format("The assembly validated successfully. {0} / {1} nodes checked out.",
validationCount, nodes.Count);
return(true);
}
else
{
message = String.Format(
"The assembly failed to validate. {0} / {1} nodes checked out. {2} parts/assemblies were not found.",
validationCount, nodes.Count, failedCount);
return(false);
}
}
private void StandaloneViewerForm_Shown(object sender, EventArgs e)
{
RigidNode_Base node = BXDJSkeleton.ReadSkeleton(LaunchParams.Path + @"\skeleton.bxdj");
List <RigidNode_Base> nodes = node.ListAllNodes();
List <BXDAMesh> meshes = new List <BXDAMesh>();
foreach (RigidNode_Base n in nodes)
{
BXDAMesh mesh = new BXDAMesh();
mesh.ReadFromFile(LaunchParams.Path + "\\" + n.ModelFileName);
if (!n.GUID.Equals(mesh.GUID))
{
MessageBox.Show(n.ModelFileName + " has been modified.", "Could not load mesh.");
}
meshes.Add(mesh);
}
robotViewer1.LoadModel(node, meshes);
robotViewer1.FixLimits();
robotViewer1.HighlightAll();
}
/// <summary>
/// Clones joint settings for matching skeletal joints from one skeleton to the other. This does not overwrite existing joint drivers.
/// </summary>
/// <param name="from">Source skeleton</param>
/// <param name="to">Destination skeleton</param>
public static void CloneDriversFromTo(RigidNode_Base from, RigidNode_Base to, bool overwrite = false)
{
List <RigidNode_Base> tempNodes = new List <RigidNode_Base>();
from.ListAllNodes(tempNodes);
Dictionary <string, RigidNode_Base> fromNodes = new Dictionary <string, RigidNode_Base>();
foreach (RigidNode_Base cpy in tempNodes)
{
fromNodes[cpy.GetModelID()] = cpy;
}
tempNodes.Clear();
to.ListAllNodes(tempNodes);
foreach (RigidNode_Base copyTo in tempNodes)
{
RigidNode_Base fromNode;
if (fromNodes.TryGetValue(copyTo.GetModelID(), out fromNode))
{
if (copyTo.GetSkeletalJoint() != null && fromNode.GetSkeletalJoint() != null && copyTo.GetSkeletalJoint().GetJointType() == fromNode.GetSkeletalJoint().GetJointType())
{
if (copyTo.GetSkeletalJoint().cDriver == null || overwrite)
{
// Swap driver.
copyTo.GetSkeletalJoint().cDriver = fromNode.GetSkeletalJoint().cDriver;
}
if (copyTo.GetSkeletalJoint().attachedSensors.Count == 0 || overwrite)
{
// Swap sensors.
copyTo.GetSkeletalJoint().attachedSensors = fromNode.GetSkeletalJoint().attachedSensors;
}
}
}
}
}
/// <summary>
/// Iterates over all the joints in the skeleton and writes the corrosponding Inventor limit into the internal joint limit
/// Necessary to pull the limits into the joint as the exporter exports. Where the joint is actually written to the .bxdj,
/// we are unable to access RobotExporterAPI or InventorRobotExporter, so writing the limits here is a workaround to that issue.
/// </summary>
/// <param name="skeleton">Skeleton to write limits to</param>
private static void WriteLimits(RigidNode_Base skeleton)
{
var nodes = new List <RigidNode_Base>();
skeleton.ListAllNodes(nodes);
var parentId = new int[nodes.Count];
for (var i = 0; i < nodes.Count; i++)
{
if (nodes[i].GetParent() != null)
{
parentId[i] = nodes.IndexOf(nodes[i].GetParent());
if (parentId[i] < 0)
{
throw new Exception("Can't resolve parent ID for " + nodes[i].ToString());
}
}
else
{
parentId[i] = -1;
}
}
for (var i = 0; i < nodes.Count; i++)
{
if (parentId[i] >= 0)
{
var inventorJoint = nodes[i].GetSkeletalJoint() as InventorSkeletalJoint;
if (inventorJoint != null)
{
inventorJoint.ReloadInventorJoint();
}
}
}
}
/// <summary>
/// Detects all the wheel nodes in a robot. Needs improvement
/// </summary>
/// <param name="baseNode"></param>
/// <param name="driveTrain"></param>
/// <param name="wheelCount"></param>
/// <returns></returns>
public static List <RigidNode_Base> DetectWheels(RigidNode_Base baseNode, WizardData.WizardDriveTrain driveTrain, int wheelCount)
{
List <RigidNode_Base> jointParentFilter = new List <RigidNode_Base>();
foreach (RigidNode_Base node in baseNode.ListAllNodes())
{
//For the first filter, we take out any nodes that do not have parents and rotational joints.
if (node.GetParent() != null && node.GetSkeletalJoint() != null && node.GetSkeletalJoint().GetJointType() == SkeletalJointType.ROTATIONAL)
{
jointParentFilter.Add(node);
}
}
//Find node with the lowest y value
RigidNode_Base lowestNode = null; double lowestY = double.MaxValue;
foreach (RigidNode_Base node in jointParentFilter)
{
if (lowestNode == null)
{
lowestNode = node;
lowestY = node.GetSkeletalJoint().GetAngularDOF().First().basePoint.y;
}
else
{
if (node.GetSkeletalJoint().GetAngularDOF().First().basePoint.y < lowestY)
{
lowestNode = node;
lowestY = node.GetSkeletalJoint().GetAngularDOF().First().basePoint.y;
}
}
}
//Find all nodes with y values within 0.1
List <RigidNode_Base> lowestNodesFilter = new List <RigidNode_Base>();
foreach (RigidNode_Base node in jointParentFilter)
{
if (node == lowestNode)
{
lowestNodesFilter.Add(node);
}
else if (lowestY - node.GetSkeletalJoint().GetAngularDOF().First().basePoint.y <= 0.1d && lowestY - node.GetSkeletalJoint().GetAngularDOF().First().basePoint.y >= -0.1d)
{
lowestNodesFilter.Add(node);
}
}
//Hopefully this will be enough filtering :point_right:
if (lowestNodesFilter.Count == wheelCount)
{
BXDJSkeleton.SetupFileNames(baseNode);
StandardAddInServer.Instance.JointEditorPane_SelectedJoint(lowestNodesFilter);
return(lowestNodesFilter);
}
//Find the nodes with the highest and lowest x values
RigidNode_Base leftMostNode = null; double highestX = double.MinValue;
RigidNode_Base rightMostNode = null; double lowestX = double.MaxValue;
foreach (RigidNode_Base node in lowestNodesFilter)
{
if (highestX == double.MinValue)
{
leftMostNode = node;
highestX = node.GetSkeletalJoint().GetAngularDOF().First().basePoint.x;
}
else if (node.GetSkeletalJoint().GetAngularDOF().First().basePoint.x > highestX)
{
leftMostNode = node;
highestX = node.GetSkeletalJoint().GetAngularDOF().First().basePoint.x;
}
if (lowestX == double.MaxValue)
{
rightMostNode = node;
lowestX = node.GetSkeletalJoint().GetAngularDOF().First().basePoint.x;
}
else if (node.GetSkeletalJoint().GetAngularDOF().First().basePoint.x < lowestX)
{
leftMostNode = node;
lowestX = node.GetSkeletalJoint().GetAngularDOF().First().basePoint.x;
}
}
//Find all the nodes with x values within 0.1 of both.
List <RigidNode_Base> rightWheels = new List <RigidNode_Base> {
rightMostNode
};
List <RigidNode_Base> leftWheels = new List <RigidNode_Base> {
leftMostNode
};
foreach (RigidNode_Base node in lowestNodesFilter)
{
if (node == leftMostNode || node == rightMostNode)
{
continue;
}
if (node.GetSkeletalJoint().GetAngularDOF().First().basePoint.x - lowestX <= 0.1d && node.GetSkeletalJoint().GetAngularDOF().First().basePoint.x - lowestX >= -0.1d)
{
rightWheels.Add(node);
}
else if (node.GetSkeletalJoint().GetAngularDOF().First().basePoint.x - highestX <= 0.1d && node.GetSkeletalJoint().GetAngularDOF().First().basePoint.x - highestX >= -0.1d)
{
leftWheels.Add(node);
}
}
string nodes = "Nodes detected: ";
List <RigidNode_Base> wheels = new List <RigidNode_Base>();
wheels.AddRange(rightWheels);
wheels.AddRange(leftWheels);
BXDJSkeleton.SetupFileNames(baseNode);
foreach (var node in wheels)
{
nodes += node.ModelFileName + ", ";
}
#region DEBUG
#if DEBUG
MessageBox.Show(nodes);
#endif
#endregion
return(wheels);
}
public SynthesisGUI()
{
InitializeComponent();
Instance = this;
robotViewer1.LoadSettings(ViewerSettings);
bxdaEditorPane1.Units = ViewerSettings.modelUnits;
RigidNode_Base.NODE_FACTORY = delegate(Guid guid)
{
return(new OGL_RigidNode(guid));
};
fileNew.Click += new System.EventHandler(delegate(object sender, System.EventArgs e)
{
SetNew();
});
fileLoad.Click += new System.EventHandler(delegate(object sender, System.EventArgs e)
{
LoadFromInventor();
});
fileOpen.Click += new System.EventHandler(delegate(object sender, System.EventArgs e)
{
OpenExisting();
});
fileSave.Click += new System.EventHandler(delegate(object sender, System.EventArgs e)
{
SaveRobot(false);
});
fileSaveAs.Click += new System.EventHandler(delegate(object sender, System.EventArgs e)
{
SaveRobot(true);
});
fileExit.Click += new System.EventHandler(delegate(object sender, System.EventArgs e)
{
Close();
});
settingsExporter.Click += new System.EventHandler(delegate(object sender, System.EventArgs e)
{
var defaultValues = BXDSettings.Instance.GetSettingsObject("Exporter Settings");
ExporterSettingsForm eSettingsForm = new ExporterSettingsForm((defaultValues != null) ? (ExporterSettingsForm.ExporterSettingsValues)defaultValues :
ExporterSettingsForm.GetDefaultSettings());
eSettingsForm.ShowDialog(this);
BXDSettings.Instance.AddSettingsObject("Exporter Settings", eSettingsForm.values);
ExporterSettings = eSettingsForm.values;
});
settingsViewer.Click += new System.EventHandler(delegate(object sender, System.EventArgs e)
{
var defaultValues = BXDSettings.Instance.GetSettingsObject("Viewer Settings");
ViewerSettingsForm vSettingsForm = new ViewerSettingsForm((defaultValues != null) ? (ViewerSettingsForm.ViewerSettingsValues)defaultValues :
ViewerSettingsForm.GetDefaultSettings());
vSettingsForm.ShowDialog(this);
BXDSettings.Instance.AddSettingsObject("Viewer Settings", vSettingsForm.values);
ViewerSettings = vSettingsForm.values;
robotViewer1.LoadSettings(ViewerSettings);
bxdaEditorPane1.Units = ViewerSettings.modelUnits;
});
helpAbout.Click += new System.EventHandler(delegate(object sender, System.EventArgs e)
{
AboutDialog about = new AboutDialog();
about.ShowDialog(this);
});
this.FormClosing += new FormClosingEventHandler(delegate(object sender, FormClosingEventArgs e)
{
if (SkeletonBase != null && !WarnUnsaved())
{
e.Cancel = true;
}
else
{
BXDSettings.Save();
}
InventorManager.ReleaseInventor();
});
jointEditorPane1.ModifiedJoint += delegate(List <RigidNode_Base> nodes)
{
if (nodes == null || nodes.Count == 0)
{
return;
}
foreach (RigidNode_Base node in nodes)
{
if (node.GetSkeletalJoint() != null && node.GetSkeletalJoint().cDriver != null &&
node.GetSkeletalJoint().cDriver.GetInfo <WheelDriverMeta>() != null &&
node.GetSkeletalJoint().cDriver.GetInfo <WheelDriverMeta>().radius == 0 &&
node is OGL_RigidNode)
{
float radius, width;
BXDVector3 center;
(node as OGL_RigidNode).GetWheelInfo(out radius, out width, out center);
WheelDriverMeta wheelDriver = node.GetSkeletalJoint().cDriver.GetInfo <WheelDriverMeta>();
wheelDriver.center = center;
wheelDriver.radius = radius;
wheelDriver.width = width;
node.GetSkeletalJoint().cDriver.AddInfo(wheelDriver);
}
}
};
jointEditorPane1.SelectedJoint += robotViewer1.SelectJoints;
jointEditorPane1.SelectedJoint += bxdaEditorPane1.SelectJoints;
robotViewer1.NodeSelected += jointEditorPane1.AddSelection;
robotViewer1.NodeSelected += bxdaEditorPane1.AddSelection;
bxdaEditorPane1.NodeSelected += (BXDAMesh mesh) =>
{
List <RigidNode_Base> nodes = new List <RigidNode_Base>();
SkeletonBase.ListAllNodes(nodes);
jointEditorPane1.AddSelection(nodes[Meshes.IndexOf(mesh)], true);
};
bxdaEditorPane1.NodeSelected += (BXDAMesh mesh) =>
{
List <RigidNode_Base> nodes = new List <RigidNode_Base>();
SkeletonBase.ListAllNodes(nodes);
robotViewer1.SelectJoints(nodes.GetRange(Meshes.IndexOf(mesh), 1));
};
}
/// <summary>
/// Repositions the robot so it is aligned at the center of the field, and resets all the
/// joints, velocities, etc..
/// </summary>
private void resetRobot()
{
if (activeRobot != null && skeleton != null)
{
unityPacket.OutputStatePacket packet = null;
var unityWheelData = new List <GameObject>();
var unityWheels = new List <UnityRigidNode>();
// Invert the position of the root object
/**/
activeRobot.transform.localPosition = new Vector3(1f, 1f, -0.5f);
activeRobot.transform.rotation = Quaternion.identity;
activeRobot.transform.localRotation = Quaternion.identity;
mainNode.transform.rotation = Quaternion.identity;
/**/
var nodes = skeleton.ListAllNodes();
foreach (RigidNode_Base node in nodes)
{
UnityRigidNode uNode = (UnityRigidNode)node;
uNode.unityObject.transform.localPosition = new Vector3(0f, 0f, -5f);
uNode.unityObject.transform.localRotation = Quaternion.identity;
if (uNode.unityObject.rigidbody != null)
{
uNode.unityObject.rigidbody.velocity = Vector3.zero;
uNode.unityObject.rigidbody.angularVelocity = Vector3.zero;
}
if (uNode.HasDriverMeta <WheelDriverMeta>() && uNode.wheelCollider != null && uNode.GetDriverMeta <WheelDriverMeta>().isDriveWheel)
{
unityWheelData.Add(uNode.wheelCollider);
unityWheels.Add(uNode);
}
}
bool isMecanum = false;
if (unityWheelData.Count > 0)
{
auxFunctions.OrientRobot(unityWheelData, activeRobot.transform);
foreach (RigidNode_Base node in nodes)
{
UnityRigidNode uNode = (UnityRigidNode)node;
if (uNode.HasDriverMeta <WheelDriverMeta>() && uNode.wheelCollider != null)
{
unityWheelData.Add(uNode.wheelCollider);
if (uNode.GetDriverMeta <WheelDriverMeta>().GetTypeString().Equals("Mecanum"))
{
isMecanum = true;
uNode.unityObject.GetComponent <BetterWheelCollider>().wheelType = (int)WheelType.MECANUM;
}
if (uNode.GetDriverMeta <WheelDriverMeta>().GetTypeString().Equals("Omni Wheel"))
{
uNode.unityObject.GetComponent <BetterWheelCollider>().wheelType = (int)WheelType.OMNI;
}
}
}
auxFunctions.rightRobot(unityWheelData, activeRobot.transform);
if (isMecanum)
{
float sumX = 0;
float sumZ = 0;
foreach (UnityRigidNode uNode in unityWheels)
{
sumX += uNode.wheelCollider.transform.localPosition.x;
sumZ += uNode.wheelCollider.transform.localPosition.z;
}
float avgX = sumX / unityWheels.Count;
float avgZ = sumZ / unityWheels.Count;
foreach (UnityRigidNode uNode in unityWheels)
{
if (uNode.unityObject.GetComponent <BetterWheelCollider>().wheelType == (int)WheelType.MECANUM)
{
if ((avgX > uNode.wheelCollider.transform.localPosition.x && avgZ < uNode.wheelCollider.transform.localPosition.z) ||
(avgX < uNode.wheelCollider.transform.localPosition.x && avgZ > uNode.wheelCollider.transform.localPosition.z))
{
uNode.unityObject.GetComponent <BetterWheelCollider>().wheelAngle = -45;
}
else
{
uNode.unityObject.GetComponent <BetterWheelCollider>().wheelAngle = 45;
}
}
}
}
}
mainNode.transform.rotation = rotation;
mainNode.rigidbody.inertiaTensorRotation = Quaternion.identity;
//makes sure robot spawns in the correct place
mainNode.transform.position = new Vector3(-2f, 1f, -3f);
mainNode.rigidbody.isKinematic = true;
StartCoroutine(FinishReset());
}
foreach (GameObject o in totes)
{
GameObject.Destroy(o);
}
totes.Clear();
}
/// <summary>
/// Writes out the skeleton file for the skeleton with the base provided to the path provided.
/// </summary>
/// <param name="path"></param>
/// <param name="baseNode"></param>
public static void WriteSkeleton(string path, RigidNode_Base baseNode)
{
//EXPERIMENT
// JSONExport(path, baseNode);
List <RigidNode_Base> nodes = new List <RigidNode_Base>();
baseNode.ListAllNodes(nodes);
// Determine the parent ID for each node in the list.
int[] parentID = new int[nodes.Count];
for (int i = 0; i < nodes.Count; i++)
{
if (nodes[i].GetParent() != null)
{
parentID[i] = nodes.IndexOf(nodes[i].GetParent());
if (parentID[i] < 0)
{
throw new Exception("Can't resolve parent ID for " + nodes[i].ToString());
}
}
else
{
parentID[i] = -1;
}
}
XmlWriterSettings settings = new XmlWriterSettings();
settings.Indent = true;
XmlWriter writer = XmlWriter.Create(path, settings);
writer.WriteStartDocument();
writer.WriteStartElement("BXDJ");
writer.WriteAttributeString("Version", BXDJ_CURRENT_VERSION);
for (int i = 0; i < nodes.Count; i++)
{
writer.WriteStartElement("Node");
writer.WriteAttributeString("GUID", nodes[i].GUID.ToString());
writer.WriteElementString("ParentID", parentID[i].ToString());
writer.WriteElementString("ModelFileName", FileUtilities.SanatizeFileName("node_" + i + ".bxda"));
writer.WriteElementString("ModelID", nodes[i].GetModelID());
if (parentID[i] >= 0)
{
WriteJoint(nodes[i].GetSkeletalJoint(), writer);
}
writer.WriteEndElement();
}
writer.WriteElementString("DriveTrainType", (baseNode.driveTrainType).ToString());
writer.WriteElementString("SoftwareExportedWith", "INVENTOR");
writer.Close();
}
/// <summary>
/// Loads a robot from file into the simulator.
/// </summary>
private void TryLoadRobot()
{
//resets rotation for new robot
rotation = Quaternion.identity;
if (activeRobot != null)
{
skeleton = null;
UnityEngine.Object.Destroy(activeRobot);
}
if (filePath != null && skeleton == null)
{
//Debug.Log (filePath);
List <Collider> meshColliders = new List <Collider>();
activeRobot = new GameObject("Robot");
activeRobot.transform.parent = transform;
List <RigidNode_Base> names = new List <RigidNode_Base>();
RigidNode_Base.NODE_FACTORY = delegate(Guid guid)
{
return(new UnityRigidNode(guid));
};
skeleton = BXDJSkeleton.ReadSkeleton(filePath + "skeleton.bxdj");
//Debug.Log(filePath + "skeleton.bxdj");
skeleton.ListAllNodes(names);
foreach (RigidNode_Base node in names)
{
UnityRigidNode uNode = (UnityRigidNode)node;
uNode.CreateTransform(activeRobot.transform);
if (!uNode.CreateMesh(filePath + uNode.ModelFileName))
{
UserMessageManager.Dispatch(node.ModelFileName + " has been modified and cannot be loaded.", 6f);
skeleton = null;
UnityEngine.Object.Destroy(activeRobot);
return;
}
uNode.CreateJoint();
Debug.Log("Joint");
meshColliders.AddRange(uNode.unityObject.GetComponentsInChildren <Collider>());
}
{ // Add some mass to the base object
UnityRigidNode uNode = (UnityRigidNode)skeleton;
uNode.unityObject.transform.rigidbody.mass += 20f * PHYSICS_MASS_MULTIPLIER; // Battery'
}
//finds main node of robot to use its rigidbody
mainNode = GameObject.Find("node_0.bxda");
//Debug.Log ("HELLO AMIREKA: " + mainNode);
auxFunctions.IgnoreCollisionDetection(meshColliders);
resetRobot();
}
else
{
Debug.Log("unityWheelData is null...");
}
HideGuiSidebar();
}
public SynthesisGUI(bool MakeOwners = false)
{
InitializeComponent();
Instance = this;
bxdaEditorPane1.Units = "lbs";
BXDAViewerPaneForm.Controls.Add(bxdaEditorPane1);
if (MakeOwners)
{
BXDAViewerPaneForm.Owner = this;
}
BXDAViewerPaneForm.FormClosing += Generic_FormClosing;
JointPaneForm.Controls.Add(jointEditorPane1);
if (MakeOwners)
{
JointPaneForm.Owner = this;
}
JointPaneForm.FormClosing += Generic_FormClosing;
RigidNode_Base.NODE_FACTORY = delegate(Guid guid)
{
return(new OGL_RigidNode(guid));
};
settingsExporter.Click += SettingsExporter_OnClick;
Shown += SynthesisGUI_Shown;
FormClosing += new FormClosingEventHandler(delegate(object sender, FormClosingEventArgs e)
{
if (SkeletonBase != null && !WarnUnsaved())
{
e.Cancel = true;
}
InventorManager.ReleaseInventor();
});
jointEditorPane1.ModifiedJoint += delegate(List <RigidNode_Base> nodes)
{
if (nodes == null || nodes.Count == 0)
{
return;
}
foreach (RigidNode_Base node in nodes)
{
if (node.GetSkeletalJoint() != null && node.GetSkeletalJoint().cDriver != null &&
node.GetSkeletalJoint().cDriver.GetInfo <WheelDriverMeta>() != null &&
node.GetSkeletalJoint().cDriver.GetInfo <WheelDriverMeta>().radius == 0 &&
node is OGL_RigidNode)
{
(node as OGL_RigidNode).GetWheelInfo(out float radius, out float width, out BXDVector3 center);
WheelDriverMeta wheelDriver = node.GetSkeletalJoint().cDriver.GetInfo <WheelDriverMeta>();
wheelDriver.center = center;
wheelDriver.radius = radius;
wheelDriver.width = width;
node.GetSkeletalJoint().cDriver.AddInfo(wheelDriver);
}
}
};
jointEditorPane1.SelectedJoint += bxdaEditorPane1.SelectJoints;
bxdaEditorPane1.NodeSelected += (BXDAMesh mesh) =>
{
List <RigidNode_Base> nodes = new List <RigidNode_Base>();
SkeletonBase.ListAllNodes(nodes);
jointEditorPane1.AddSelection(nodes[Meshes.IndexOf(mesh)], true);
};
}
/// <summary>
/// Open a previously exported robot.
/// </summary>
/// <param name="validate">If it is not null, this will validate the open inventor assembly.</param>
public bool OpenExisting(ValidationAction validate, bool warnUnsaved = false)
{
if (SkeletonBase != null && warnUnsaved && !WarnUnsaved())
{
return(false);
}
string dirPath = OpenFolderPath();
if (dirPath == null)
{
return(false);
}
try
{
List <RigidNode_Base> nodes = new List <RigidNode_Base>();
SkeletonBase = BXDJSkeleton.ReadSkeleton(dirPath + "\\skeleton.bxdj");
if (validate != null)
{
if (!validate(SkeletonBase, out string message))
{
while (true)
{
DialogResult result = MessageBox.Show(message, "Assembly Validation", MessageBoxButtons.AbortRetryIgnore, MessageBoxIcon.Error, MessageBoxDefaultButton.Button1);
if (result == DialogResult.Retry)
{
continue;
}
if (result == DialogResult.Abort)
{
return(false);
}
break;
}
}
#region DEBUG
#if DEBUG
else
{
MessageBox.Show(message);
}
#endif
#endregion
}
SkeletonBase.ListAllNodes(nodes);
Meshes = new List <BXDAMesh>();
foreach (RigidNode_Base n in nodes)
{
BXDAMesh mesh = new BXDAMesh();
mesh.ReadFromFile(dirPath + "\\" + n.ModelFileName);
if (!n.GUID.Equals(mesh.GUID))
{
MessageBox.Show(n.ModelFileName + " has been modified.", "Could not load mesh.");
return(false);
}
Meshes.Add(mesh);
}
}
catch (Exception e)
{
MessageBox.Show(e.ToString());
}
RMeta.UseSettingsDir = false;
RMeta.ActiveDir = dirPath;
RMeta.ActiveRobotName = dirPath.Split(new char[] { '\\' }, StringSplitOptions.RemoveEmptyEntries).Last();
ReloadPanels();
return(true);
}
public static void UpdateAllMotors(RigidNode_Base skeleton, UnityPacket.OutputStatePacket.DIOModule[] dioModules, int controlIndex, bool mecanum)
{
bool IsMecanum = mecanum;
int reverse = -1;
float[] pwm = new float[10];
float[] can = new float[10];
if (dioModules[0] != null)
{
pwm = dioModules[0].pwmValues;
can = dioModules[0].canValues;
}
if (IsMecanum)
{
pwm[(int)MecanumPorts.FRONT_RIGHT] +=
(InputControl.GetButton(Controls.buttons[controlIndex].forward) ? reverse * SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].backward) ? reverse * -SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].left) ? reverse * -SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].right) ? reverse * SPEED_ARROW_PWM : 0.0f) +
(Input.GetKey(KeyCode.O) ? reverse * SPEED_ARROW_PWM : 0.0f) + //Left Rotate
(Input.GetKey(KeyCode.P) ? reverse * -SPEED_ARROW_PWM : 0.0f); //Right Rotate
pwm[(int)MecanumPorts.BACK_LEFT] +=
(InputControl.GetButton(Controls.buttons[controlIndex].forward) ? SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].backward) ? -SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].left) ? -SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].right) ? SPEED_ARROW_PWM : 0.0f) +
(Input.GetKey(KeyCode.O) ? -SPEED_ARROW_PWM : 0.0f) + //Left Rotate
(Input.GetKey(KeyCode.P) ? SPEED_ARROW_PWM : 0.0f); //Right Rotate
pwm[(int)MecanumPorts.FRONT_LEFT] +=
(InputControl.GetButton(Controls.buttons[controlIndex].forward) ? SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].backward) ? -SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].left) ? SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].right) ? -SPEED_ARROW_PWM : 0.0f) +
(Input.GetKey(KeyCode.O) ? -SPEED_ARROW_PWM : 0.0f) + //Left Rotate
(Input.GetKey(KeyCode.P) ? SPEED_ARROW_PWM : 0.0f); //Right Rotate
pwm[(int)MecanumPorts.BACK_RIGHT] +=
(InputControl.GetButton(Controls.buttons[controlIndex].forward) ? reverse * SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].backward) ? reverse * -SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].left) ? reverse * SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].right) ? reverse * -SPEED_ARROW_PWM : 0.0f) +
(Input.GetKey(KeyCode.O) ? reverse * SPEED_ARROW_PWM : 0.0f) + //Left Rotate
(Input.GetKey(KeyCode.P) ? reverse * -SPEED_ARROW_PWM : 0.0f); //Right Rotate
}
else
{
pwm[0] +=
(InputControl.GetButton(Controls.buttons[controlIndex].forward) ? -SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].backward) ? SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].left) ? -SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].right) ? SPEED_ARROW_PWM : 0.0f);
pwm[1] +=
(InputControl.GetButton(Controls.buttons[controlIndex].forward) ? SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].backward) ? -SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].left) ? -SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].right) ? SPEED_ARROW_PWM : 0.0f);
pwm[2] +=
(InputControl.GetButton(Controls.buttons[controlIndex].pwm2Plus)) ? SPEED_ARROW_PWM :
(InputControl.GetButton(Controls.buttons[controlIndex].pwm2Neg)) ? -SPEED_ARROW_PWM : 0f;
pwm[3] +=
(InputControl.GetButton(Controls.buttons[controlIndex].pwm3Plus)) ? SPEED_ARROW_PWM :
(InputControl.GetButton(Controls.buttons[controlIndex].pwm3Neg)) ? -SPEED_ARROW_PWM : 0f;
pwm[4] +=
(InputControl.GetButton(Controls.buttons[controlIndex].pwm4Plus)) ? SPEED_ARROW_PWM :
(InputControl.GetButton(Controls.buttons[controlIndex].pwm4Neg)) ? -SPEED_ARROW_PWM : 0f;
pwm[5] +=
(InputControl.GetButton(Controls.buttons[controlIndex].pwm5Plus)) ? SPEED_ARROW_PWM :
(InputControl.GetButton(Controls.buttons[controlIndex].pwm5Neg)) ? -SPEED_ARROW_PWM : 0f;
pwm[6] +=
(InputControl.GetButton(Controls.buttons[controlIndex].pwm6Plus)) ? SPEED_ARROW_PWM :
(InputControl.GetButton(Controls.buttons[controlIndex].pwm6Neg)) ? -SPEED_ARROW_PWM : 0f;
}
List <RigidNode_Base> listOfSubNodes = new List <RigidNode_Base>();
skeleton.ListAllNodes(listOfSubNodes);
for (int i = 0; i < pwm.Length; i++)
{
foreach (RigidNode_Base node in listOfSubNodes)
{
RigidNode rigidNode = (RigidNode)node;
if (rigidNode.GetSkeletalJoint() != null && rigidNode.GetSkeletalJoint().cDriver != null)
{
if (rigidNode.GetSkeletalJoint().cDriver.GetDriveType().IsMotor())
{
if (rigidNode.GetSkeletalJoint().cDriver.portA == i + 1)
{
float maxSpeed = 0f;
float impulse = 0f;
float friction = 0f;
if (rigidNode.HasDriverMeta <WheelDriverMeta>())
{
maxSpeed = WHEEL_MAX_SPEED;
impulse = WHEEL_MOTOR_IMPULSE;
friction = WHEEL_COAST_FRICTION;
}
else
{
maxSpeed = HINGE_MAX_SPEED;
impulse = HINGE_MOTOR_IMPULSE;
friction = HINGE_COAST_FRICTION;
}
BHingedConstraint hingedConstraint = rigidNode.MainObject.GetComponent <BHingedConstraint>();
hingedConstraint.enableMotor = true;
hingedConstraint.targetMotorAngularVelocity = pwm[i] > 0f ? maxSpeed : pwm[i] < 0f ? -maxSpeed : 0f;
hingedConstraint.maxMotorImpulse = pwm[i] == 0f ? friction : Mathf.Abs(pwm[i] * impulse);
}
}
else if (rigidNode.GetSkeletalJoint().cDriver.GetDriveType().IsElevator())
{
if (rigidNode.GetSkeletalJoint().cDriver.portA == i + 1 && rigidNode.HasDriverMeta <ElevatorDriverMeta>())
{
BSliderConstraint bSliderConstraint = rigidNode.MainObject.GetComponent <BSliderConstraint>();
SliderConstraint sc = (SliderConstraint)bSliderConstraint.GetConstraint();
sc.PoweredLinearMotor = true;
sc.MaxLinearMotorForce = MAX_SLIDER_FORCE;
sc.TargetLinearMotorVelocity = pwm[i] * MAX_SLIDER_SPEED;
}
}
}
}
}
}