public void RemovePhysicsObjects()
{
AgX_Assembly.RemoveFromSim();
/* foreach(Joint lockjoint in locks)
* {
* if(lockjoint.agxJoint != null)
* lockjoint.agxJoint.Remove();
* lockjoint.agxJoint = null;
* }*/
locks.Clear();
locks = null;
/* foreach (Module mod in modules)
* {
* mod.joint.agxJoint.Remove();mod.joint.agxJoint = null;
* mod.frames[0].agxFrame.Remove();mod.frames[0].agxFrame = null;
* mod.frames[1].agxFrame.Remove();mod.frames[1].agxFrame = null;
* }*/
modules.Clear();
modules = null;
/* foreach(SensorModule mod in sensorModules)
* {
* mod.agxPrimitive.Remove();mod.agxPrimitive = null;
* }*/
sensorModules.Clear();
sensorModules = null;
}
public void ChangeInitRot(AgX_Interface.Quaternion rot)
{
if (!simulation_Running)
{
AgX_Assembly.SetRotation(rot);
robot.Update();
Update_Vis(robot);
RobotStartRotation = rot;
}
}
public void ChangeInitPos(AgX_Interface.Vector3 pos)
{
if (!simulation_Running)
{
AgX_Assembly.SetPosition(pos);
robot.Update();
Update_Vis(robot);
RobotStartPosition = pos;
}
}
public void Initialize()//Initializes lock joints.
{
//AgX_Assembly.AddToSim();
//Creates joints between frames and inits frame objects:
foreach (Module mod in modules)
{
mod.Initialize();
}
foreach (SensorModule mod in sensorModules)
{
mod.Initialize();
}
int lockNrCount = 0;
//Sets locks between modules:(ITERATES CURRENT MODULE)->
for (int i = 0; i < modules.Count; i++)
{
if (sensorModules.Any(x => x.rightMod_Nr == i))//[]>X | if this module is to the right of a sensor moduleif(sensorModules.Any(x => x.rightMod_Nr == modules[i].number)
{
locks[lockNrCount].Create_SensorModuleLock(sensorModules.Find(x => x.rightMod_Nr == i), modules[i].frames[0]);
lockNrCount++;
if (!sensorModules.Any(x => x.leftMod_Nr == i) && i + 1 < modules.Count)//X->X | if there is no sensorModule to the right, and there IS a module to the right
{
locks[lockNrCount].Create_Lock(modules[i].frames[1], modules[i + 1].frames[0]);
lockNrCount++;
}
}
if (sensorModules.Any(x => x.leftMod_Nr == i))//X<-[] | if this module is to the left of a sensor module:
{
locks[lockNrCount].Create_SensorModuleLock(modules[i].frames[1], sensorModules.Find(x => x.leftMod_Nr == i));
lockNrCount++;
}
if (i + 1 < modules.Count && !sensorModules.Any(x => x.leftMod_Nr == i || x.rightMod_Nr == i))//If the iterator is not exceeding module count, and this module and has no right or left to any sensor
{
locks[lockNrCount].Create_Lock(modules[i].frames[1], modules[i + 1].frames[0]);
lockNrCount++;
}
}
AgX_Assembly.AddToSim();
}
