//IMPORTANT Time.fixedTime must match the timestep being used here.
public static List <UnityEngine.Vector3> SimulateBall(BRigidBody ballRb, UnityEngine.Vector3 ballThrowForce, int numberOfSimulationSteps, bool reverseOrder)
{
var ballPositions = new List <UnityEngine.Vector3>(numberOfSimulationSteps);
//Create a World
Debug.Log("Initialize physics");
CollisionConfiguration CollisionConf;
CollisionDispatcher Dispatcher;
BroadphaseInterface Broadphase;
CollisionWorld cw;
ConstraintSolver Solver;
BulletSharp.SoftBody.SoftBodyWorldInfo softBodyWorldInfo;
//This should create a copy of the BPhysicsWorld with the same settings
BPhysicsWorld bw = BPhysicsWorld.Get();
bw.CreatePhysicsWorld(out cw, out CollisionConf, out Dispatcher, out Broadphase, out Solver, out softBodyWorldInfo);
World = (DiscreteDynamicsWorld)cw;
//Copy all existing rigidbodies in scene
// IMPORTANT rigidbodies must be added to the offline world in the same order that they are in the source world
// this is because collisions must be resolved in the same order for the sim to be deterministic
DiscreteDynamicsWorld sourceWorld = (DiscreteDynamicsWorld)bw.world;
BulletSharp.RigidBody bulletBallRb = null;
BulletSharp.Math.Matrix mm = BulletSharp.Math.Matrix.Identity;
for (int i = 0; i < sourceWorld.NumCollisionObjects; i++)
{
CollisionObject co = sourceWorld.CollisionObjectArray[i];
if (co != null && co.UserObject is BRigidBody)
{
BRigidBody rb = (BRigidBody)co.UserObject;
float mass = rb.isDynamic() ? rb.mass : 0f;
BCollisionShape existingShape = rb.GetComponent <BCollisionShape>();
CollisionShape shape = null;
if (existingShape is BSphereShape)
{
shape = ((BSphereShape)existingShape).CopyCollisionShape();
}
else if (existingShape is BBoxShape)
{
shape = ((BBoxShape)existingShape).CopyCollisionShape();
}
RigidBody bulletRB = null;
BulletSharp.Math.Vector3 localInertia = new BulletSharp.Math.Vector3();
rb.CreateOrConfigureRigidBody(ref bulletRB, ref localInertia, shape, null);
BulletSharp.Math.Vector3 pos = rb.GetCollisionObject().WorldTransform.Origin;
BulletSharp.Math.Quaternion rot = rb.GetCollisionObject().WorldTransform.GetOrientation();
BulletSharp.Math.Matrix.AffineTransformation(1f, ref rot, ref pos, out mm);
bulletRB.WorldTransform = mm;
World.AddRigidBody(bulletRB, rb.groupsIBelongTo, rb.collisionMask);
if (rb == ballRb)
{
bulletBallRb = bulletRB;
bulletRB.ApplyCentralImpulse(ballThrowForce.ToBullet());
}
}
}
//Step the simulation numberOfSimulationSteps times
for (int i = 0; i < numberOfSimulationSteps; i++)
{
int numSteps = World.StepSimulation(1f / 60f, 10, 1f / 60f);
ballPositions.Add(bulletBallRb.WorldTransform.Origin.ToUnity());
}
UnityEngine.Debug.Log("ExitPhysics");
if (World != null)
{
//remove/dispose constraints
int i;
for (i = World.NumConstraints - 1; i >= 0; i--)
{
TypedConstraint constraint = World.GetConstraint(i);
World.RemoveConstraint(constraint);
constraint.Dispose();
}
//remove the rigidbodies from the dynamics world and delete them
for (i = World.NumCollisionObjects - 1; i >= 0; i--)
{
CollisionObject obj = World.CollisionObjectArray[i];
RigidBody body = obj as RigidBody;
if (body != null && body.MotionState != null)
{
body.MotionState.Dispose();
}
World.RemoveCollisionObject(obj);
obj.Dispose();
}
World.Dispose();
Broadphase.Dispose();
Dispatcher.Dispose();
CollisionConf.Dispose();
}
if (Broadphase != null)
{
Broadphase.Dispose();
}
if (Dispatcher != null)
{
Dispatcher.Dispose();
}
if (CollisionConf != null)
{
CollisionConf.Dispose();
}
return(ballPositions);
}
public bool CreateMesh(string filePath)
{
BXDAMesh mesh = new BXDAMesh();
mesh.ReadFromFile(filePath);
if (!mesh.GUID.Equals(GUID))
{
return(false);
}
List <GameObject> meshObjects = new List <GameObject>();
AuxFunctions.ReadMeshSet(mesh.meshes, delegate(int id, BXDAMesh.BXDASubMesh sub, Mesh meshu)
{
GameObject meshObject = new GameObject(MainObject.name + "_mesh");
meshObjects.Add(meshObject);
meshObject.AddComponent <MeshFilter>().mesh = meshu;
meshObject.AddComponent <MeshRenderer>();
Material[] materials = new Material[meshu.subMeshCount];
for (int i = 0; i < materials.Length; i++)
{
materials[i] = sub.surfaces[i].AsMaterial(true);
}
meshObject.GetComponent <MeshRenderer>().materials = materials;
meshObject.transform.position = root.position;
meshObject.transform.rotation = root.rotation;
ComOffset = meshObject.transform.GetComponent <MeshFilter>().mesh.bounds.center;
});
Mesh[] colliders = new Mesh[mesh.colliders.Count];
AuxFunctions.ReadMeshSet(mesh.colliders, delegate(int id, BXDAMesh.BXDASubMesh sub, Mesh meshu)
{
colliders[id] = meshu;
});
MainObject.transform.position = root.position + ComOffset;
MainObject.transform.rotation = root.rotation;
foreach (GameObject meshObject in meshObjects)
{
meshObject.transform.parent = MainObject.transform;
}
if (!this.HasDriverMeta <WheelDriverMeta>() || this.GetDriverMeta <WheelDriverMeta>().type == WheelType.NOT_A_WHEEL)
{
BMultiHullShape hullShape = MainObject.AddComponent <BMultiHullShape>();
foreach (Mesh collider in colliders)
{
ConvexHullShape hull = new ConvexHullShape(Array.ConvertAll(collider.vertices, x => x.ToBullet()), collider.vertices.Length);
hull.Margin = 0f;
hullShape.AddHullShape(hull, BulletSharp.Math.Matrix.Translation(-ComOffset.ToBullet()));
}
PhysicalProperties = mesh.physics;
BRigidBody rigidBody = MainObject.AddComponent <BRigidBody>();
rigidBody.mass = mesh.physics.mass;
rigidBody.friction = 0.25f;
rigidBody.RemoveOnCollisionCallbackEventHandler();
((RigidBody)rigidBody.GetCollisionObject()).ActivationState = ActivationState.DisableDeactivation;
foreach (BRigidBody rb in MainObject.transform.parent.GetComponentsInChildren <BRigidBody>())
{
rigidBody.GetCollisionObject().SetIgnoreCollisionCheck(rb.GetCollisionObject(), true);
}
MainObject.AddComponent <BMultiCallbacks>().AddCallback((StateMachine.Instance.CurrentState as MainState).CollisionTracker);
}
if (this.HasDriverMeta <WheelDriverMeta>() && this.GetDriverMeta <WheelDriverMeta>().type != WheelType.NOT_A_WHEEL && GetParent() == null)
{
BRigidBody rigidBody = MainObject.GetComponent <BRigidBody>();
if (MixAndMatchMode.isMixAndMatchMode)
{
rigidBody.mass += PlayerPrefs.GetFloat("wheelMass", 1f);
}
rigidBody.GetCollisionObject().CollisionShape.CalculateLocalInertia(rigidBody.mass);
}
#region Free mesh
foreach (var list in new List <BXDAMesh.BXDASubMesh>[] { mesh.meshes, mesh.colliders })
{
foreach (BXDAMesh.BXDASubMesh sub in list)
{
sub.verts = null;
sub.norms = null;
foreach (BXDAMesh.BXDASurface surf in sub.surfaces)
{
surf.indicies = null;
}
}
for (int i = 0; i < list.Count; i++)
{
list[i] = null;
}
}
mesh = null;
GC.Collect(GC.MaxGeneration, GCCollectionMode.Forced);
#endregion
return(true);
}
/// <summary>
/// Updates the positions and rotations of each tracker's parent object according to the replay time.
/// </summary>
public override void LateUpdate()
{
if (dynamicCamera == null)
{
dynamicCamera = UnityEngine.Object.FindObjectOfType <DynamicCamera>();
camera = dynamicCamera.GetComponent <UnityEngine.Camera>();
}
if (Input.InputControl.GetButtonDown(Controls.buttons[0].cameraToggle))
{
dynamicCamera.ToggleCameraState(dynamicCamera.ActiveState);
}
if (firstFrame)
{
firstFrame = false;
}
else if (UnityEngine.Input.GetKeyDown(KeyCode.Space))
{
if (UnityEngine.Input.GetKey(KeyCode.LeftControl) || UnityEngine.Input.GetKey(KeyCode.RightControl))
{
rewindTime = Tracker.Lifetime;
playbackMode = PlaybackMode.Play;
}
else
{
switch (playbackMode)
{
case PlaybackMode.Paused:
if (rewindTime == 0f)
{
rewindTime = Tracker.Lifetime;
}
playbackMode = PlaybackMode.Play;
break;
case PlaybackMode.Play:
case PlaybackMode.Rewind:
playbackMode = PlaybackMode.Paused;
break;
}
}
}
switch (playbackMode)
{
case PlaybackMode.Rewind:
rewindTime += Time.deltaTime;
break;
case PlaybackMode.Play:
rewindTime -= Time.deltaTime;
break;
}
if (UnityEngine.Input.GetKey(KeyCode.LeftArrow))
{
rewindTime += Time.deltaTime * (UnityEngine.Input.GetKey(KeyCode.LeftControl) || UnityEngine.Input.GetKey(KeyCode.RightControl) ? 0.05f : 0.25f);
playbackMode = PlaybackMode.Paused;
}
if (UnityEngine.Input.GetKey(KeyCode.RightArrow))
{
rewindTime -= Time.deltaTime * (UnityEngine.Input.GetKey(KeyCode.LeftControl) || UnityEngine.Input.GetKey(KeyCode.RightControl) ? 0.05f : 0.25f);
playbackMode = PlaybackMode.Paused;
}
if (rewindTime < 0.0f)
{
rewindTime = 0.0f;
playbackMode = PlaybackMode.Paused;
}
else if (rewindTime > Tracker.Lifetime)
{
rewindTime = Tracker.Lifetime;
playbackMode = PlaybackMode.Paused;
}
foreach (Tracker t in trackers)
{
float replayTime = RewindFrame;
int currentIndex = (int)Math.Floor(replayTime);
StateDescriptor lowerState = t.States[currentIndex];
StateDescriptor upperState = currentIndex < t.States.Length - 1 ? t.States[currentIndex + 1] : lowerState;
float percent = replayTime - currentIndex;
BRigidBody rb = t.GetComponent <BRigidBody>();
if (!rb.GetCollisionObject().IsActive)
{
rb.GetCollisionObject().Activate();
}
rb.SetPosition(BulletSharp.Math.Vector3.Lerp(lowerState.Position, upperState.Position, percent).ToUnity());
rb.SetRotation(BulletSharp.Math.Matrix.Lerp(lowerState.Rotation, upperState.Rotation, percent).GetOrientation().ToUnity());
}
}
public bool CreateMesh(string filePath, bool isMixAndMatch = false, float wheelMass = 1.0f)
{
//Debug.Log(filePath);
BXDAMesh mesh = new BXDAMesh();
mesh.ReadFromFile(filePath);
//if (!mesh.GUID.Equals(GUID))
//{
// Debug.Log("Returning false");
// return false;
//}
List <GameObject> meshObjects = new List <GameObject>();
AuxFunctions.ReadMeshSet(mesh.meshes, delegate(int id, BXDAMesh.BXDASubMesh sub, Mesh meshu)
{
GameObject meshObject = new GameObject(MainObject.name + "_mesh");
meshObjects.Add(meshObject);
meshObject.AddComponent <MeshFilter>().mesh = meshu;
meshObject.AddComponent <MeshRenderer>();
Material[] materials = new Material[meshu.subMeshCount];
for (int i = 0; i < materials.Length; i++)
{
materials[i] = sub.surfaces[i].AsMaterial(true);
}
meshObject.GetComponent <MeshRenderer>().materials = materials;
meshObject.transform.position = root.position;
meshObject.transform.rotation = root.rotation;
}, true);
Vector3 com = mesh.physics.centerOfMass.AsV3();
com.x *= -1;
ComOffset = com;
Mesh[] colliders = new Mesh[mesh.colliders.Count];
AuxFunctions.ReadMeshSet(mesh.colliders, delegate(int id, BXDAMesh.BXDASubMesh sub, Mesh meshu)
{
colliders[id] = meshu;
}, true);
MainObject.transform.position = root.position + ComOffset;
MainObject.transform.rotation = root.rotation;
foreach (GameObject meshObject in meshObjects)
{
meshObject.transform.parent = MainObject.transform;
}
if (!this.HasDriverMeta <WheelDriverMeta>() || this.GetDriverMeta <WheelDriverMeta>().type == WheelType.NOT_A_WHEEL)
{
BMultiHullShape hullShape = MainObject.AddComponent <BMultiHullShape>();
foreach (Mesh collider in colliders)
{
//Mesh m = AuxFunctions.GenerateCollisionMesh(collider, Vector3.zero, 0f/*CollisionMargin*/);
//ConvexHullShape hull = new ConvexHullShape(Array.ConvertAll(m.vertices, x => x.ToBullet()), m.vertices.Length);
//hull.Margin = CollisionMargin;
//hullShape.AddHullShape(hull, BulletSharp.Math.Matrix.Translation(-ComOffset.ToBullet()));
ConvexHullShape hull = new ConvexHullShape(Array.ConvertAll(collider.vertices, x => x.ToBullet()), collider.vertices.Length);
hull.Margin = CollisionMargin;
hullShape.AddHullShape(hull, BulletSharp.Math.Matrix.Translation(-ComOffset.ToBullet()));
}
MainObject.AddComponent <MeshRenderer>();
PhysicalProperties = mesh.physics;
//Debug.Log(PhysicalProperties.centerOfMass);
BRigidBody rigidBody = MainObject.AddComponent <BRigidBody>();
rigidBody.mass = mesh.physics.mass;
rigidBody.friction = 0.25f;
rigidBody.linearSleepingThreshold = LinearSleepingThreshold;
rigidBody.angularSleepingThreshold = AngularSleepingThreshold;
rigidBody.RemoveOnCollisionCallbackEventHandler();
foreach (BRigidBody rb in MainObject.transform.parent.GetComponentsInChildren <BRigidBody>())
{
rigidBody.GetCollisionObject().SetIgnoreCollisionCheck(rb.GetCollisionObject(), true);
}
MainObject.AddComponent <BMultiCallbacks>().AddCallback((StateMachine.Instance.CurrentState as MainState).CollisionTracker);
}
if (this.HasDriverMeta <WheelDriverMeta>() && this.GetDriverMeta <WheelDriverMeta>().type != WheelType.NOT_A_WHEEL && GetParent() == null)
{
BRigidBody rigidBody = MainObject.GetComponent <BRigidBody>();
if (isMixAndMatch)
{
rigidBody.mass += wheelMass;
}
rigidBody.GetCollisionObject().CollisionShape.CalculateLocalInertia(rigidBody.mass);
}
#region Free mesh
foreach (var list in new List <BXDAMesh.BXDASubMesh>[] { mesh.meshes, mesh.colliders })
{
foreach (BXDAMesh.BXDASubMesh sub in list)
{
sub.verts = null;
sub.norms = null;
foreach (BXDAMesh.BXDASurface surf in sub.surfaces)
{
surf.indicies = null;
}
}
for (int i = 0; i < list.Count; i++)
{
list[i] = null;
}
}
mesh = null;
GC.Collect(GC.MaxGeneration, GCCollectionMode.Forced);
#endregion
return(true);
}
public bool CreateMesh(string filePath)
{
BXDAMesh mesh = new BXDAMesh();
mesh.ReadFromFile(filePath);
if (!mesh.GUID.Equals(GUID))
return false;
List<GameObject> meshObjects = new List<GameObject>();
AuxFunctions.ReadMeshSet(mesh.meshes, delegate (int id, BXDAMesh.BXDASubMesh sub, Mesh meshu)
{
GameObject meshObject = new GameObject(MainObject.name + "_mesh");
meshObjects.Add(meshObject);
meshObject.AddComponent<MeshFilter>().mesh = meshu;
meshObject.AddComponent<MeshRenderer>();
Material[] materials = new Material[meshu.subMeshCount];
for (int i = 0; i < materials.Length; i++)
materials[i] = sub.surfaces[i].AsMaterial(true);
meshObject.GetComponent<MeshRenderer>().materials = materials;
meshObject.transform.position = root.position;
meshObject.transform.rotation = root.rotation;
ComOffset = meshObject.transform.GetComponent<MeshFilter>().mesh.bounds.center;
});
Mesh[] colliders = new Mesh[mesh.colliders.Count];
AuxFunctions.ReadMeshSet(mesh.colliders, delegate (int id, BXDAMesh.BXDASubMesh sub, Mesh meshu)
{
colliders[id] = meshu;
});
MainObject.transform.position = root.position + ComOffset;
MainObject.transform.rotation = root.rotation;
foreach (GameObject meshObject in meshObjects)
meshObject.transform.parent = MainObject.transform;
if (this.HasDriverMeta<WheelDriverMeta>())
{
CreateWheel();
}
else
{
BMultiHullShape hullShape = MainObject.AddComponent<BMultiHullShape>();
foreach (Mesh collider in colliders)
{
ConvexHullShape hull = new ConvexHullShape(Array.ConvertAll(collider.vertices, x => x.ToBullet()), collider.vertices.Length);
hull.Margin = 0f;
hullShape.AddHullShape(hull, BulletSharp.Math.Matrix.Translation(-ComOffset.ToBullet()));
}
}
physicalProperties = mesh.physics;
BRigidBody rigidBody = MainObject.AddComponent<BRigidBody>();
rigidBody.mass = mesh.physics.mass;
rigidBody.friction = 1f;
if (this.HasDriverMeta<WheelDriverMeta>())
UpdateWheelRigidBody();
foreach (BRigidBody rb in MainObject.transform.parent.GetComponentsInChildren<BRigidBody>())
{
rigidBody.GetCollisionObject().SetIgnoreCollisionCheck(rb.GetCollisionObject(), true);
}
if (this.HasDriverMeta<WheelDriverMeta>())
UpdateWheelMass(); // 'tis a wheel, so needs more mass for joints to work correctly.
#region Free mesh
foreach (var list in new List<BXDAMesh.BXDASubMesh>[] { mesh.meshes, mesh.colliders })
{
foreach (BXDAMesh.BXDASubMesh sub in list)
{
sub.verts = null;
sub.norms = null;
foreach (BXDAMesh.BXDASurface surf in sub.surfaces)
{
surf.indicies = null;
}
}
for (int i = 0; i < list.Count; i++)
{
list[i] = null;
}
}
mesh = null;
GC.Collect(GC.MaxGeneration, GCCollectionMode.Forced);
#endregion
return true;
}
internal override bool _BuildConstraint()
{
BPhysicsWorld world = BPhysicsWorld.Get();
if (m_constraintPtr != null && m_isInWorld && world != null)
{
m_isInWorld = false;
world.RemoveConstraint(m_constraintPtr);
}
BRigidBody targetRigidBodyA = GetComponent <BRigidBody>();
if (targetRigidBodyA == null)
{
Debug.LogError("Fixed Constraint needs to be added to a component with a BRigidBody.");
return(false);
}
if (!targetRigidBodyA.isInWorld)
{
world.AddRigidBody(targetRigidBodyA);
}
RigidBody rba = (RigidBody)targetRigidBodyA.GetCollisionObject();
if (rba == null)
{
Debug.LogError("Constraint could not get bullet RigidBody from target rigid body");
return(false);
}
if (m_otherRigidBody == null)
{
Debug.LogError("Other rigid body is not set");
return(false);
}
if (!m_otherRigidBody.isInWorld)
{
world.AddRigidBody(m_otherRigidBody);
}
RigidBody rbb = (RigidBody)m_otherRigidBody.GetCollisionObject();
if (rbb == null)
{
Debug.LogError("Constraint could not get bullet RigidBody from target rigid body");
return(false);
}
initialTransform = BM.Matrix.AffineTransformation(1f, localRotationOffset.ToBullet(), m_localConstraintPoint.ToBullet());
m_constraintPtr = new FixedConstraint(rba, rbb, BM.Matrix.Identity, initialTransform)
{
Userobject = this,
DebugDrawSize = m_debugDrawSize,
BreakingImpulseThreshold = m_breakingImpulseThreshold,
OverrideNumSolverIterations = m_overrideNumSolverIterations
};
return(true);
}
/// <summary>
/// Rotates the robot about its origin by a mathematical 4x4 matrix
/// </summary>
public void RotateRobot(BulletSharp.Math.Matrix rotationMatrix)
{
BulletSharp.Math.Vector3?origin = null;
foreach (RigidNode n in rootNode.ListAllNodes())
{
BRigidBody br = n.MainObject.GetComponent <BRigidBody>();
if (br == null)
{
continue;
}
RigidBody r = (RigidBody)br.GetCollisionObject();
if (origin == null)
{
origin = r.CenterOfMassPosition;
}
BulletSharp.Math.Matrix rotationTransform = new BulletSharp.Math.Matrix();
rotationTransform.Basis = rotationMatrix;
rotationTransform.Origin = BulletSharp.Math.Vector3.Zero;
BulletSharp.Math.Matrix currentTransform = r.WorldTransform;
BulletSharp.Math.Vector3 pos = currentTransform.Origin;
currentTransform.Origin -= origin.Value;
currentTransform *= rotationTransform;
currentTransform.Origin += origin.Value;
r.WorldTransform = currentTransform;
}
if (RobotHasManipulator)
{
foreach (RigidNode n in manipulatorNode.ListAllNodes())
{
BRigidBody br = n.MainObject.GetComponent <BRigidBody>();
if (br == null)
{
continue;
}
RigidBody r = (RigidBody)br.GetCollisionObject();
if (origin == null)
{
origin = r.CenterOfMassPosition;
}
BulletSharp.Math.Matrix rotationTransform = new BulletSharp.Math.Matrix();
rotationTransform.Basis = rotationMatrix;
rotationTransform.Origin = BulletSharp.Math.Vector3.Zero;
BulletSharp.Math.Matrix currentTransform = r.WorldTransform;
BulletSharp.Math.Vector3 pos = currentTransform.Origin;
currentTransform.Origin -= origin.Value;
currentTransform *= rotationTransform;
currentTransform.Origin += origin.Value;
r.WorldTransform = currentTransform;
}
}
}
/// <summary>
/// Return the robot to robotStartPosition and destroy extra game pieces
/// </summary>
/// <param name="resetTransform"></param>
public void BeginReset()
{
BRigidBody rigidBody = GetComponentInChildren <BRigidBody>();
if (rigidBody != null && !rigidBody.GetCollisionObject().IsActive)
{
rigidBody.GetCollisionObject().Activate();
}
if (!mainState.DynamicCameraObject.GetComponent <DynamicCamera>().cameraState.GetType().Equals(typeof(DynamicCamera.ConfigurationState)))
{
Debug.Log(mainState.DynamicCameraObject.GetComponent <DynamicCamera>().cameraState);
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;
}
if (RobotHasManipulator && RobotIsMixAndMatch)
{
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++;
}
}
//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!");
}
}
else
{
UserMessageManager.Dispatch("Please don't reset robot during configuration!", 5f);
}
}
/// <summary>
/// Builds the constraint.
/// </summary>
/// <returns></returns>
internal override bool _BuildConstraint()
{
BPhysicsWorld world = BPhysicsWorld.Get();
if (m_constraintPtr != null)
{
if (m_isInWorld && world != null)
{
m_isInWorld = false;
world.RemoveConstraint(m_constraintPtr);
}
}
BRigidBody targetRigidBodyA = GetComponent <BRigidBody>();
if (targetRigidBodyA == null)
{
Debug.LogError("BHingedConstraint needs to be added to a component with a BRigidBody.");
return(false);
}
if (!targetRigidBodyA.isInWorld)
{
world.AddRigidBody(targetRigidBodyA);
}
if (m_localConstraintAxisX == Vector3.zero)
{
Debug.LogError("Constaint axis cannot be zero vector");
return(false);
}
RigidBody rba = (RigidBody)targetRigidBodyA.GetCollisionObject();
if (rba == null)
{
Debug.LogError("Constraint could not get bullet RigidBody from target rigid body");
return(false);
}
if (m_constraintType == ConstraintType.constrainToAnotherBody)
{
if (m_otherRigidBody == null)
{
Debug.LogError("Other rigid body must not be null");
return(false);
}
if (!m_otherRigidBody.isInWorld)
{
world.AddRigidBody(m_otherRigidBody);
}
RigidBody rbb = (RigidBody)m_otherRigidBody.GetCollisionObject();
if (rbb == null)
{
Debug.LogError("Constraint could not get bullet RigidBody from target rigid body");
return(false);
}
m_constraintPtr = new HingeConstraint(rba, rbb, m_localConstraintPoint.ToBullet(),
m_otherRigidBody.transform.InverseTransformPoint(transform.TransformPoint(m_localConstraintPoint)).ToBullet(),
m_axisInA.ToBullet(), m_axisInB.ToBullet());
}
else
{
m_constraintPtr = new HingeConstraint(rba, m_localConstraintPoint.ToBullet(), m_axisInA.ToBullet(), false);
}
if (m_enableMotor)
{
((HingeConstraint)m_constraintPtr).EnableAngularMotor(true, m_targetMotorAngularVelocity, m_maxMotorImpulse);
}
if (m_setLimit)
{
((HingeConstraint)m_constraintPtr).SetLimit(m_lowLimitAngleRadians, m_highLimitAngleRadians, m_limitSoftness, m_limitBiasFactor);
}
m_constraintPtr.Userobject = this;
m_constraintPtr.DebugDrawSize = m_debugDrawSize;
m_constraintPtr.BreakingImpulseThreshold = m_breakingImpulseThreshold;
m_constraintPtr.OverrideNumSolverIterations = m_overrideNumSolverIterations;
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;
float[] can;
if (dioModules[0] != null)
{
pwm = dioModules[0].pwmValues;
can = dioModules[0].canValues;
}
else
{
pwm = new float[10];
can = new float[10];
}
if (IsMecanum)
{
#region Mecanum Drive
pwm[(int)MecanumPorts.FRONT_RIGHT] +=
(InputControl.GetAxis(Controls.axes[controlIndex].vertical) * -SPEED_ARROW_PWM) +
(InputControl.GetAxis(Controls.axes[controlIndex].horizontal) * -SPEED_ARROW_PWM) +
(InputControl.GetAxis(Controls.axes[controlIndex].pwm2Axes) * -SPEED_ARROW_PWM);
pwm[(int)MecanumPorts.FRONT_LEFT] +=
(InputControl.GetAxis(Controls.axes[controlIndex].vertical) * SPEED_ARROW_PWM) +
(InputControl.GetAxis(Controls.axes[controlIndex].horizontal) * SPEED_ARROW_PWM) +
(InputControl.GetAxis(Controls.axes[controlIndex].pwm2Axes) * -SPEED_ARROW_PWM);
//For some reason, giving the back wheels 0.25 power instead of 0.5 works for strafing
pwm[(int)MecanumPorts.BACK_RIGHT] +=
(InputControl.GetAxis(Controls.axes[controlIndex].vertical) * -SPEED_ARROW_PWM) +
(InputControl.GetAxis(Controls.axes[controlIndex].horizontal) * -SPEED_ARROW_PWM) +
(InputControl.GetAxis(Controls.axes[controlIndex].pwm2Axes) * 0.25f);
pwm[(int)MecanumPorts.BACK_LEFT] +=
(InputControl.GetAxis(Controls.axes[controlIndex].vertical) * SPEED_ARROW_PWM) +
(InputControl.GetAxis(Controls.axes[controlIndex].horizontal) * SPEED_ARROW_PWM) +
(InputControl.GetAxis(Controls.axes[controlIndex].pwm2Axes) * 0.25f);
pwm[4] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm4Axes) * SPEED_ARROW_PWM);
pwm[5] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm5Axes) * SPEED_ARROW_PWM);
pwm[6] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm6Axes) * SPEED_ARROW_PWM);
#endregion
}
if (Controls.TankDriveEnabled)
{
#region Tank Drive
//pwm[0] +=
// (InputControl.GetButton(Controls.buttons[controlIndex].tankFrontLeft) ? SPEED_ARROW_PWM : 0.0f) +
// (InputControl.GetButton(Controls.buttons[controlIndex].tankBackLeft) ? -SPEED_ARROW_PWM : 0.0f);
//pwm[1] +=
// (InputControl.GetButton(Controls.buttons[controlIndex].tankFrontRight) ? -SPEED_ARROW_PWM : 0.0f) +
// (InputControl.GetButton(Controls.buttons[controlIndex].tankBackRight) ? SPEED_ARROW_PWM : 0.0f);
pwm[0] +=
(InputControl.GetAxis(Controls.axes[controlIndex].tankRightAxes) * SPEED_ARROW_PWM);
pwm[1] +=
(InputControl.GetAxis(Controls.axes[controlIndex].tankLeftAxes) * SPEED_ARROW_PWM);
pwm[2] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm2Axes) * SPEED_ARROW_PWM);
pwm[3] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm3Axes) * SPEED_ARROW_PWM);
pwm[4] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm4Axes) * SPEED_ARROW_PWM);
pwm[5] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm5Axes) * SPEED_ARROW_PWM);
pwm[6] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm6Axes) * SPEED_ARROW_PWM);
#endregion
}
else
{
#region Arcade Drive
//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[0] +=
(InputControl.GetAxis(Controls.axes[controlIndex].vertical) * -SPEED_ARROW_PWM) +
(InputControl.GetAxis(Controls.axes[controlIndex].horizontal) * SPEED_ARROW_PWM);
pwm[1] +=
(InputControl.GetAxis(Controls.axes[controlIndex].vertical) * SPEED_ARROW_PWM) +
(InputControl.GetAxis(Controls.axes[controlIndex].horizontal) * SPEED_ARROW_PWM);
pwm[2] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm2Axes) * SPEED_ARROW_PWM);
pwm[3] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm3Axes) * SPEED_ARROW_PWM);
pwm[4] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm4Axes) * SPEED_ARROW_PWM);
pwm[5] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm5Axes) * SPEED_ARROW_PWM);
pwm[6] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm6Axes) * SPEED_ARROW_PWM);
#endregion
}
listOfSubNodes.Clear();
skeleton.ListAllNodes(listOfSubNodes);
for (int i = 0; i < pwm.Length; i++)
{
foreach (RigidNode_Base node in listOfSubNodes)
{
RigidNode rigidNode = (RigidNode)node;
if (pwm[i] != 0f)
{
BRigidBody rigidBody = rigidNode.MainObject.GetComponent <BRigidBody>();
if (rigidBody != null && !rigidBody.GetCollisionObject().IsActive)
{
rigidBody.GetCollisionObject().Activate();
}
}
BRaycastWheel raycastWheel = rigidNode.MainObject.GetComponent <BRaycastWheel>();
if (raycastWheel != null)
{
if (rigidNode.GetSkeletalJoint().cDriver.portA == i + 1)
{
raycastWheel.ApplyForce(pwm[i]);
}
}
if (rigidNode.GetSkeletalJoint() != null && rigidNode.GetSkeletalJoint().cDriver != null)
{
if (rigidNode.GetSkeletalJoint().cDriver.GetDriveType().IsMotor() && rigidNode.MainObject.GetComponent <BHingedConstraint>() != null)
{
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;
}
}
}
}
}
}
/// <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())
{
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 hasManipulator = PlayerPrefs.GetInt("hasManipulator"); //0 is false, 1 is true
int isMixAndMatch = PlayerPrefs.GetInt("MixAndMatch"); // 0 is false, 1 is true
if (hasManipulator == 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!");
}
}
