/// <summary>
/// Called when the Tracker is initialized.
/// </summary>
protected override void Awake()
{
base.Awake();
physicsWorld = BPhysicsWorld.Get();
rigidBody = (RigidBody)GetComponent <BRigidBody>().GetCollisionObject();
States = new FixedQueue <StateDescriptor>(Length, StateDescriptor);
BPhysicsTickListener.Instance.OnTick += AddState;
}
private void ClickRuler()
{
//Casts a ray from the camera in the direction the mouse is in and returns the closest object hit
Ray ray = Camera.main.ScreenPointToRay(Input.mousePosition);
BulletSharp.Math.Vector3 start = ray.origin.ToBullet();
BulletSharp.Math.Vector3 end = ray.GetPoint(200).ToBullet();
//Creates a callback result that will be updated if we do a ray test with it
ClosestRayResultCallback rayResult = new ClosestRayResultCallback(ref start, ref end);
//Retrieves the bullet physics world and does a ray test with the given coordinates and updates the callback object
BPhysicsWorld world = BPhysicsWorld.Get();
world.world.RayTest(start, end, rayResult);
if (rayResult.CollisionObject != null)
{
if (Input.GetMouseButtonDown(0))
{
if (firstPoint == BulletSharp.Math.Vector3.Zero)
{
rulerStartPoint.GetComponent <LineRenderer>().enabled = true;
rulerStartPoint.GetComponent <LineRenderer>().SetPosition(0, rulerStartPoint.transform.position);
rulerEndPoint.SetActive(true);
firstPoint = rayResult.HitPointWorld;
}
else
{
DisableRuler();
}
}
if (firstPoint != null)
{
Debug.DrawRay(firstPoint.ToUnity(), Vector3.up);
}
if (firstPoint == BulletSharp.Math.Vector3.Zero)
{
rulerStartPoint.transform.position = rayResult.HitPointWorld.ToUnity();
}
else
{
rulerText.text = Mathf.Round(BulletSharp.Math.Vector3.Distance(firstPoint, rayResult.HitPointWorld) * 328.084f) / 100 + "ft";
rulerXText.text = Mathf.Round(Mathf.Abs(firstPoint.X - rayResult.HitPointWorld.X) * 328.084f) / 100 + "ft";
rulerYText.text = Mathf.Round(Mathf.Abs(firstPoint.Y - rayResult.HitPointWorld.Y) * 328.084f) / 100 + "ft";
rulerZText.text = Mathf.Round(Mathf.Abs(firstPoint.Z - rayResult.HitPointWorld.Z) * 328.084f) / 100 + "ft";
rulerEndPoint.transform.position = rayResult.HitPointWorld.ToUnity();
rulerStartPoint.GetComponent <LineRenderer>().SetPosition(1, rulerEndPoint.transform.position);
}
}
}
/// <summary>
/// Called after Awake() when the script instance is enabled.
/// Initializes variables then loads the field and robot as well as setting up replay features.
/// </summary>
public override void Start()
{
//getting bullet physics information
physicsWorld = BPhysicsWorld.Get();
((DynamicsWorld)physicsWorld.world).SetInternalTickCallback(BRobotManager.Instance.UpdateRaycastRobots);
lastFrameCount = physicsWorld.frameCount;
//setting up replay
Trackers = new List <Tracker>();
CollisionTracker = new CollisionTracker(this);
//starts a new instance of unity packet which receives packets from the driver station
unityPacket = new UnityPacket();
unityPacket.Start();
//loads all the controls
Controls.Load();
//If a replay has been selected, load the replay. Otherwise, load the field and robot.
string selectedReplay = PlayerPrefs.GetString("simSelectedReplay");
SpawnedRobots = new List <Robot>();
if (string.IsNullOrEmpty(selectedReplay))
{
Tracking = true;
Debug.Log(LoadField(PlayerPrefs.GetString("simSelectedField")) ? "Load field success!" : "Load field failed.");
Debug.Log(LoadRobot(PlayerPrefs.GetString("simSelectedRobot")) ? "Load robot success!" : "Load robot failed.");
int isMixAndMatch = PlayerPrefs.GetInt("MixAndMatch", 0); // 0 is false, 1 is true
int hasManipulator = PlayerPrefs.GetInt("hasManipulator");
if (isMixAndMatch == 1 && hasManipulator == 1)
{
Debug.Log(LoadManipulator(PlayerPrefs.GetString("simSelectedManipulator")) ? "Load manipulator success" : "Load manipulator failed");
}
}
else
{
awaitingReplay = true;
LoadReplay(selectedReplay);
}
//initializes the dynamic camera
dynamicCameraObject = GameObject.Find("Main Camera");
dynamicCamera = dynamicCameraObject.AddComponent <DynamicCamera>();
DynamicCamera.MovingEnabled = true;
sensorManager = GameObject.Find("SensorManager").GetComponent <SensorManager>();
sensorManagerGUI = GameObject.Find("StateMachine").GetComponent <SensorManagerGUI>();
robotCameraManager = GameObject.Find("RobotCameraList").GetComponent <RobotCameraManager>();
}
/// <summary>
/// Called when the script instance is being initialized.
/// Initializes the bullet physics environment
/// </summary>
public override void Awake()
{
Environment.SetEnvironmentVariable("MONO_REFLECTION_SERIALIZER", "yes");
GImpactCollisionAlgorithm.RegisterAlgorithm((CollisionDispatcher)BPhysicsWorld.Get().world.Dispatcher);
//BPhysicsWorld.Get().DebugDrawMode = DebugDrawModes.DrawWireframe | DebugDrawModes.DrawConstraints | DebugDrawModes.DrawConstraintLimits;
BPhysicsWorld.Get().DebugDrawMode = DebugDrawModes.All;
BPhysicsWorld.Get().DoDebugDraw = false;
((DynamicsWorld)BPhysicsWorld.Get().world).SolverInfo.NumIterations = SolverIterations;
CollisionTracker = new CollisionTracker(this);
unityPacket = new UnityPacket();
SpawnedRobots = new List <SimulatorRobot>();
}
public void ExecuteBlastJump()
{
blastJumpToken = false;
handledThisFrame = true;
if (!timer.CheckAndReset())
{
return;
}
BulletSharp.Math.Vector3 from = startRaycastFrom.position.ToBullet();
BulletSharp.Math.Vector3 to = (startRaycastFrom.position + startRaycastFrom.forward * maxBlastJumpRange).ToBullet();
ClosestRayResultCallback callback = new ClosestRayResultCallback(ref from, ref to);
BPhysicsWorld.Get().world.RayTest(from, to, callback);
if (enableDebugMode)
{
Debug.DrawLine(from.ToUnity(), to.ToUnity(), Color.green, 2f);
}
if (callback.HasHit)
{
float force = 0;
Vector3 meToGround = callback.HitPointWorld.ToUnity() - transform.position;
float dist = meToGround.magnitude;
if (dist < maxBlastJumpRange)
{
//Falloff curve visual: https://www.desmos.com/calculator/plvrrosegp
var b = 1f / (Mathf.Pow(maxBlastJumpRange, 2) * smallestBlastForce);
force = blastJumpForce / (1 * maxBlastJumpForce + (falloffStrength * Mathf.Abs(dist)) + (b * Mathf.Pow(Mathf.Abs(dist), 2)));
}
rigidBody.AddImpulse(-meToGround.normalized * force);
#region Debug
if (enableDebugMode)
{
Debug.Log("Blasting off with a force of: " + force);
var s = GameObject.CreatePrimitive(PrimitiveType.Sphere);
s.transform.localScale = Vector3.one * .1f;
s.transform.position = callback.HitPointWorld.ToUnity();
s.GetComponent <MeshRenderer>().material.color = Color.green;
}
#endregion
}
}
public void Update()
{
if (Time.frameCount == 10)
{
BulletSharp.Math.Vector3 fromP = transform.position.ToBullet();
BulletSharp.Math.Vector3 toP = (transform.position + Vector3.down * 10f).ToBullet();
ClosestRayResultCallback callback = new ClosestRayResultCallback(ref fromP, ref toP);
BPhysicsWorld world = BPhysicsWorld.Get();
world.world.RayTest(fromP, toP, callback);
if (callback.HasHit)
{
Debug.LogFormat("Hit p={0} n={1} obj{2}", callback.HitPointWorld, callback.HitNormalWorld, callback.CollisionObject.UserObject);
}
}
}
public override float ReturnOutput()
{
//Raycasting begins, draw a ray from emitter to the receiver
Ray ray = new Ray(Emitter.transform.position, Emitter.transform.forward);
BulletSharp.Math.Vector3 fromUltra = ray.origin.ToBullet();
BulletSharp.Math.Vector3 toCollider = ray.GetPoint(10).ToBullet();
Vector3 toColliderUnity = toCollider.ToUnity();
//Callback returns all hit point results in order to avoid non colliders interfere with the ray test
AllHitsRayResultCallback raysCallback = new AllHitsRayResultCallback(fromUltra, toCollider);
//Retrieves bullet physics world and does a ray test with the given coordinates and updates the callback object
BPhysicsWorld world = BPhysicsWorld.Get();
world.world.RayTest(fromUltra, toCollider, raysCallback);
List <BulletSharp.Math.Vector3> colliderPositions = raysCallback.HitPointWorld;
BulletSharp.Math.Vector3 colliderPosition = BulletSharp.Math.Vector3.Zero;
//Set the initial distance as the distance between emitter and receiver
float distanceToCollider = sensorOffset;
//Loop through all hitpoints (exclude the origin), if there is at least one hitpoint less than the distance between two sensors,
//something should block the beam between emitter and receiver
foreach (BulletSharp.Math.Vector3 pos in colliderPositions)
{
if ((pos - fromUltra).Length < distanceToCollider && !pos.Equals(BulletSharp.Math.Vector3.Zero))
{
distanceToCollider = (pos - fromUltra).Length;
colliderPosition = pos;
}
}
//Again if the line connects to the middle of the field nothing is blocking the beam
Debug.DrawLine(fromUltra.ToUnity(), colliderPosition.ToUnity(), Color.blue);
if (distanceToCollider < sensorOffset)
{
//Something is there
return(1);
}
else
{
//Nothing in between
return(0);
}
}
/// <summary>
/// Called when the Tracker is initialized.
/// </summary>
void Awake()
{
mainState = StateMachine.Instance.FindState <MainState>();
if (mainState == null)
{
Destroy(this);
}
physicsWorld = BPhysicsWorld.Get();
rigidBody = (RigidBody)GetComponent <BRigidBody>().GetCollisionObject();
States = new FixedQueue <StateDescriptor>(Length, State);
BPhysicsTickListener.Instance.OnTick += AddState;
}
/// <summary>
/// Allows the user to select a dynamic object with their mouse and add it to the list of gamepieces.
/// </summary>
public void SetGamepiece(int index)
{
//Casts a ray from the camera in the direction the mouse is in and returns the closest object hit
Ray ray = Camera.main.ScreenPointToRay(Input.mousePosition);
BulletSharp.Math.Vector3 start = ray.origin.ToBullet();
BulletSharp.Math.Vector3 end = ray.GetPoint(200).ToBullet();
//Creates a callback result that will be updated if we do a ray test with it
ClosestRayResultCallback rayResult = new ClosestRayResultCallback(ref start, ref end);
//Retrieves the bullet physics world and does a ray test with the given coordinates and updates the callback object
BPhysicsWorld world = BPhysicsWorld.Get();
world.world.RayTest(start, end, rayResult);
//If there is a collision object and it is dynamic and not a robot part, change the gamepiece to that
if (rayResult.CollisionObject != null)
{
string name = (rayResult.CollisionObject.UserObject.ToString().Replace(" (BulletUnity.BRigidBody)", ""));
Debug.Log(name);
if (rayResult.CollisionObject.CollisionFlags == BulletSharp.CollisionFlags.StaticObject)
{
UserMessageManager.Dispatch("The gamepiece must be a dynamic object!", 3);
}
else if (GameObject.Find(name) == null)
{
Debug.Log("DPM: Game object not found");
}
else if (GameObject.Find(name).transform.parent == transform)
{
UserMessageManager.Dispatch("You cannot select a robot part as a gamepiece!", 3);
}
else
{
gamepieceNames[index] = name.Replace("(Clone)", ""); //gets rid of the clone tag given to spawned gamepieces
intakeInteractor[index].SetKeyword(gamepieceNames[index], index);
GameObject gamepiece = GameObject.Find(name);
UserMessageManager.Dispatch(name + " has been selected as the gamepiece", 2);
addingGamepiece = false;
}
}
else
{
}
}
private void SelectingNode()
{
//Casts a ray from the camera in the direction the mouse is in and returns the closest object hit
Ray ray = Camera.main.ScreenPointToRay(Input.mousePosition);
BulletSharp.Math.Vector3 start = ray.origin.ToBullet();
BulletSharp.Math.Vector3 end = ray.GetPoint(200).ToBullet();
//Creates a callback result that will be updated if we do a ray test with it
ClosestRayResultCallback rayResult = new ClosestRayResultCallback(ref start, ref end);
//Retrieves the bullet physics world and does a ray test with the given coordinates and updates the callback object
BPhysicsWorld world = BPhysicsWorld.Get();
world.world.RayTest(start, end, rayResult);
//If there is a collision object and it is dynamic and not a robot part, change the gamepiece to that
if (rayResult.CollisionObject != null)
{
string name = (rayResult.CollisionObject.UserObject.ToString().Replace(" (BulletUnity.BRigidBody)", ""));
Debug.Log(name);
if (rayResult.CollisionObject.CollisionFlags == BulletSharp.CollisionFlags.StaticObject)
{
RevertNodeColors(hoveredNode, hoveredColors);
}
else if (GameObject.Find(name) == null)
{
Debug.Log("DPM: Game object not found");
RevertNodeColors(hoveredNode, hoveredColors);
}
else if (GameObject.Find(name).transform.parent == transform)
{
if (hoveredNode != GameObject.Find(name))
{
RevertNodeColors(hoveredNode, hoveredColors);
}
hoveredNode = GameObject.Find(name);
ChangeNodeColors(hoveredNode, hoverColor, hoveredColors);
}
else
{
RevertNodeColors(hoveredNode, hoveredColors);
}
}
}
/// <summary>
/// Called after Awake() when the script instance is enabled.
/// Initializes variables then loads the field and robot as well as setting up replay features.
/// </summary>
public override void Start()
{
//getting bullet physics information
physicsWorld = BPhysicsWorld.Get();
lastFrameCount = physicsWorld.frameCount;
//setting up replay
Trackers = new List <Tracker>();
contactPoints = new FixedQueue <List <ContactDescriptor> >(Tracker.Length);
//starts a new instance of unity packet which receives packets from the driver station
unityPacket = new UnityPacket();
unityPacket.Start();
//loads all the controls
Controls.Load();
//If a replay has been selected, load the replay. Otherwise, load the field and robot.
string selectedReplay = PlayerPrefs.GetString("simSelectedReplay");
SpawnedRobots = new List <Robot>();
if (string.IsNullOrEmpty(selectedReplay))
{
tracking = true;
Debug.Log(LoadField(PlayerPrefs.GetString("simSelectedField")) ? "Load field success!" : "Load field failed.");
Debug.Log(LoadRobot(PlayerPrefs.GetString("simSelectedRobot")) ? "Load robot success!" : "Load robot failed.");
int isMixAndMatch = PlayerPrefs.GetInt("MixAndMatch", 1); //0 is true, 1 is false
if (isMixAndMatch == 0 && MixAndMatchMode.hasManipulator)
{
Debug.Log(LoadManipulator(PlayerPrefs.GetString("simSelectedManipulator")) ? "Load manipulator success" : "Load manipulator failed");
}
}
else
{
awaitingReplay = true;
LoadReplay(selectedReplay);
}
//initializes the dynamic camera
dynamicCameraObject = GameObject.Find("Main Camera");
dynamicCamera = dynamicCameraObject.AddComponent <DynamicCamera>();
DynamicCamera.MovingEnabled = true;
}
//Where the magic happens
public void PlayerStep(CollisionWorld collisionWorld, float dt)
{
wasOnGround = OnGround;
//Doesn't support sideways gravity, also flipping gravity upside down might not work great.
verticalVelocity -= (-BPhysicsWorld.Get().gravity.y + gravityModifier) * dt;
Matrix xform = ghostObject.WorldTransform;
var movementDir = BulletSharp.Math.Vector3.Zero;
//TODO: Implement more movement.
StepForwardAndStrafe(collisionWorld, ref movementDir);
xform.Origin = currentPosition;
ghostObject.WorldTransform = xform;
}
/// <summary>
/// Called when the Tracker is initialized.
/// </summary>
void Awake()
{
physicsWorld = BPhysicsWorld.Get();
rigidBody = (RigidBody)GetComponent <BRigidBody>().GetCollisionObject();
States = new FixedQueue <StateDescriptor>(Length, State);
MainState mainState = StateMachine.Instance.CurrentState as MainState;
if (mainState != null)
{
mainState.Trackers.Add(this);
}
else
{
Destroy(this);
}
}
/// <summary>
/// Updates constraint information for the active object, if applicable.
/// </summary>
private void Update()
{
if (Input.GetMouseButtonDown(0) && (Input.GetKey(KeyCode.LeftAlt) || Input.GetKey(KeyCode.RightAlt)) && constraint == null)
{
Ray ray = Camera.main.ScreenPointToRay(Input.mousePosition);
BulletSharp.Math.Vector3 start = ray.origin.ToBullet();
BulletSharp.Math.Vector3 end = ray.GetPoint(100).ToBullet();
ClosestRayResultCallback callback = new ClosestRayResultCallback(ref start, ref end);
BPhysicsWorld.Get().world.RayTest(start, end, callback);
rayDistance = (start - callback.HitPointWorld).Length;
if (callback.CollisionObject != null)
{
BRigidBody rigidBody = callback.CollisionObject.UserObject as BRigidBody;
if (rigidBody != null && rigidBody.mass > 0f)
{
initialState = rigidBody.GetCollisionObject().ActivationState;
rigidBody.GetCollisionObject().ActivationState = ActivationState.DisableDeactivation;
initialDamping = rigidBody.angularDamping;
rigidBody.angularDamping = 0.9f;
constraint = rigidBody.gameObject.AddComponent <BBallSocketConstraintEx>();
constraint.PivotInA = rigidBody.transform.InverseTransformPoint(callback.HitPointWorld.ToUnity()).ToBullet();
constraint.constraintType = BTypedConstraint.ConstraintType.constrainToPointInSpace;
}
}
}
else if (Input.GetMouseButtonUp(0) && constraint != null)
{
constraint.thisRigidBody.GetCollisionObject().ActivationState = initialState;
constraint.GetComponent <BRigidBody>().angularDamping = initialDamping;
Destroy(constraint);
constraint = null;
}
else if (constraint != null)
{
Ray ray = Camera.main.ScreenPointToRay(Input.mousePosition);
constraint.PivotInB = ray.GetPoint(rayDistance).ToBullet();
}
}
public bool AddConstraintToMultiBody(BMultiBody mb, int linkId)
{
if (mb == null)
{
throw new ArgumentNullException(nameof(mb));
}
multiBody = mb;
this.linkId = linkId;
constraint = SetupConstraint();
if (constraint == null)
{
return(false);
}
BPhysicsWorld.Get().AddMultiBodyConstraint(constraint);
return(true);
}
// Update is called once per frame
void FixedUpdate()
{
if (Time.frameCount - startFrame == 50 && !simulationStarted)
{
ballRigidbody.gameObject.SetActive(true);
bulletWorld.AddRigidBody(ballRigidbody);
simulationStarted = true;
startFrame = BPhysicsWorld.Get().frameCount;
//first simulation ==============================
ballPositionsOfflineSim = OfflineBallSimulation.SimulateBall(ballRigidbody, ballThrowImpulse, numberOfSimulationSteps, false);
//Second simulation =====================
ballRigidbody.AddImpulse(ballThrowImpulse);
for (int i = 0; i < ballPositionsOfflineSim.Count; i++)
{
Instantiate <GameObject>(ballGhostPrefab).transform.position = ballPositionsOfflineSim[i];
}
}
else if (simulationStarted && ballPositionsRealtime.Count < 500)
{
ballPositionsRealtime.Add(ballRigidbody.GetCollisionObject().WorldTransform.Origin.ToUnity());
}
if (ballPositionsRealtime.Count == 500)
{
/*
* for (int i = 0; i < ballPositionsRealtime.Count; i++)
* {
* if (ballPositionsRealtime[i] != ballPositionsOfflineSim[i])
* {
* Instantiate<GameObject>(ballGhostPrefab).transform.position = ballPositionsRealtime[i];
* Debug.LogWarning("Diverged " + ballPositionsRealtime[i].ToString("f7") + " " + ballPositionsOfflineSim[i].ToString("f7"));
* }
* }
*/
//prevent this clause from executing again
ballPositionsRealtime.Add(Vector3.zero);
}
}
// Update is called once per frame
void Update()
{
if (ammo > 0 && Input.GetMouseButtonDown(0))
{
Ray ray = cam.ScreenPointToRay(Input.mousePosition);
BulletSharp.Math.Vector3 fromP = ray.origin.ToBullet();
BulletSharp.Math.Vector3 toP = (ray.direction * 500).ToBullet();
ClosestRayResultCallback callback = new ClosestRayResultCallback(ref fromP, ref toP);
BPhysicsWorld world = BPhysicsWorld.Get();
world.world.RayTest(fromP, toP, callback);
if (callback.HasHit)
{
if (callback.CollisionObject.UserObject.GetType().Name == "BRigidBody")
{
((BRigidBody)callback.CollisionObject.UserObject).AddImpulse((new Vector3(callback.HitNormalWorld.X, callback.HitNormalWorld.Y, callback.HitNormalWorld.Z)) * -5);
}
}
ammo--;
UpdateAmmo();
}
}
/// <summary>
/// Called when the script instance is being initialized.
/// Initializes the bullet physics environment
/// </summary>
public override void Awake()
{
QualitySettings.SetQualityLevel(PlayerPrefs.GetInt("qualityLevel"));
Screen.fullScreenMode = (FullScreenMode)PlayerPrefs.GetInt("fullscreen", 1);
GameObject.Find("VersionNumber").GetComponent <Text>().text = "Version " + CurrentVersion;
if (CheckConnection())
{
WebClient client = new WebClient();
ServicePointManager.ServerCertificateValidationCallback = MyRemoteCertificateValidationCallback;
var json = new WebClient().DownloadString("https://raw.githubusercontent.com/Autodesk/synthesis/master/VersionManager.json");
VersionManager update = JsonConvert.DeserializeObject <VersionManager>(json);
SimUI.updater = update.URL;
var localVersion = new Version(CurrentVersion);
var globalVersion = new Version(update.Version);
var check = localVersion.CompareTo(globalVersion);
if (check < 0)
{
Auxiliary.FindGameObject("UpdatePrompt").SetActive(true);
}
}
robotDirectory = PlayerPrefs.GetString("RobotDirectory");
Environment.SetEnvironmentVariable("MONO_REFLECTION_SERIALIZER", "yes");
GImpactCollisionAlgorithm.RegisterAlgorithm((CollisionDispatcher)BPhysicsWorld.Get().world.Dispatcher);
//BPhysicsWorld.Get().DebugDrawMode = DebugDrawModes.DrawWireframe | DebugDrawModes.DrawConstraints | DebugDrawModes.DrawConstraintLimits;
BPhysicsWorld.Get().DebugDrawMode = DebugDrawModes.All;
BPhysicsWorld.Get().DoDebugDraw = false;
((DynamicsWorld)BPhysicsWorld.Get().world).SolverInfo.NumIterations = SolverIterations;
CollisionTracker = new CollisionTracker(this);
SpawnedRobots = new List <SimulatorRobot>();
}
public static CollisionObject ScreenPointToRay(Camera cam, Vector3 inputScreenCoords, CollisionFilterGroups rayFilterGroup, CollisionFilterGroups rayFilterMask)
{
/* Returns the first CollisionObject the ray hits.
* Requires as Input:
* - Camera cam, UnityCamera from which the ray should be cast, e.g. Camera.main
* - Vector3 inputScreenCoords, the screenpoint through which the ray should be cast. E.g. mousepointer Input.mousePosition
* - CollisionFilterGroups rayFilterGroup, of which FilterGroup the ray belongs to
* - CollisionFilterGroups rayFilterMask, which FilterMask the ray has (to map to other Objects FilterGroups
* Be aware the Bulletphysics probably needs Group/Mask to match in both ways, i.e. Ray needs to collide with otherObj, as otherObj needs to collide with Ray.
* To get all Ray hits, see commented out Callback AllHitsRayResultCallback below.
* First version. Adapt to your needs. Refer to Bulletphysics.org for info. Make Pull Request to Phong BulletUnity with improvements.*/
CollisionWorld collisionWorld = BPhysicsWorld.Get().world;
BulletSharp.Math.Vector3 rayFrom = cam.transform.position.ToBullet();
BulletSharp.Math.Vector3 rayTo = cam.ScreenToWorldPoint(new Vector3(inputScreenCoords.x, inputScreenCoords.y, cam.farClipPlane)).ToBullet();
ClosestRayResultCallback rayCallBack = new ClosestRayResultCallback(ref rayFrom, ref rayTo);
rayCallBack.CollisionFilterGroup = (short)rayFilterGroup;
rayCallBack.CollisionFilterMask = (short)rayFilterMask;
//BulletSharp.AllHitsRayResultCallback rayCallBack = new BulletSharp.AllHitsRayResultCallback(rayFrom, rayTo);
//Debug.Log("Casting ray from: " + rayFrom + " to: " + rayTo);
collisionWorld.RayTest(rayFrom, rayTo, rayCallBack);
closestRayResultReturnObj = null;
if (rayCallBack.HasHit)
{
//Debug.Log("rayCallBack " + rayCallBack.GetType() + " had a hit: " + rayCallBack.CollisionObject.UserObject + " / of type: " + rayCallBack.CollisionObject.UserObject.GetType());
closestRayResultReturnObj = rayCallBack.CollisionObject;
}
rayCallBack.Dispose();
return(closestRayResultReturnObj);
}
/// <summary>
/// Initializes the BRaycastVehicle.
/// </summary>
private void Awake()
{
rigidBody = GetComponent <BRigidBody>();
if (rigidBody == null)
{
Destroy(this);
return;
}
RaycastRobot = new RaycastRobot(defaultVehicleTuning = new VehicleTuning
{
MaxSuspensionForce = 1000f,
MaxSuspensionTravelCm = SuspensionToleranceCm,
SuspensionDamping = 10f,
SuspensionCompression = SuspensionCompressionRatio,
SuspensionStiffness = CalculateStiffness(DefaultNumWheels),
FrictionSlip = 2f
},
(RigidBody)rigidBody.GetCollisionObject(),
new BRobotRaycaster((DynamicsWorld)BPhysicsWorld.Get().world));
BRobotManager.Instance.RegisterRaycastRobot(RaycastRobot);
}
/// <summary>
/// When user click left mouse, use raycast to select a node for attachment
/// </summary>
public void SetNode()
{
if (Input.GetMouseButtonDown(0))
{
//Casts a ray from the camera in the direction the mouse is in and returns the closest object hit
Ray ray = Camera.main.ScreenPointToRay(Input.mousePosition);
BulletSharp.Math.Vector3 start = ray.origin.ToBullet();
BulletSharp.Math.Vector3 end = ray.GetPoint(200).ToBullet();
//Creates a callback result that will be updated if we do a ray test with it
ClosestRayResultCallback rayResult = new ClosestRayResultCallback(ref start, ref end);
//Retrieves the bullet physics world and does a ray test with the given coordinates and updates the callback object
BPhysicsWorld world = BPhysicsWorld.Get();
world.world.RayTest(start, end, rayResult);
Debug.Log("Selected:" + rayResult.CollisionObject);
//If there is a collision object and it is a robot part, set that to be new attachment point
if (rayResult.CollisionObject != null)
{
GameObject selectedObject = ((BRigidBody)rayResult.CollisionObject.UserObject).gameObject;
if (selectedObject.transform.parent != null && selectedObject.transform.parent.name == "Robot")
{
string name = selectedObject.name;
SelectedNode = selectedObject;
UserMessageManager.Dispatch(name + " has been selected as the node for sensor attachment", 5);
}
else
{
UserMessageManager.Dispatch("Please select a robot node", 3);
}
}
}
}
//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);
}
/// <summary>
/// Called after Awake() when the script instance is enabled.
/// Initializes variables then loads the field and robot as well as setting up replay features.
/// </summary>
public override void Start()
{
AppModel.ClearError();
//getting bullet physics information
physicsWorld = BPhysicsWorld.Get();
((DynamicsWorld)physicsWorld.world).SetInternalTickCallback(BPhysicsTickListener.Instance.PhysicsTick);
lastFrameCount = physicsWorld.frameCount;
//setting up raycast robot tick callback
BPhysicsTickListener.Instance.OnTick -= BRobotManager.Instance.UpdateRaycastRobots;
BPhysicsTickListener.Instance.OnTick += BRobotManager.Instance.UpdateRaycastRobots;
//starts a new instance of unity packet which receives packets from the driver station
unityPacket.Start();
//If a replay has been selected, load the replay. Otherwise, load the field and robot.
string selectedReplay = PlayerPrefs.GetString("simSelectedReplay");
if (string.IsNullOrEmpty(selectedReplay))
{
Tracking = true;
if (!LoadField(PlayerPrefs.GetString("simSelectedField")))
{
AppModel.ErrorToMenu("Could not load field: " + PlayerPrefs.GetString("simSelectedField") + "\nHas it been moved or deleted?)");
return;
}
if (!LoadRobot(PlayerPrefs.GetString("simSelectedRobot"), RobotTypeManager.IsMixAndMatch))
{
AppModel.ErrorToMenu("Could not load robot: " + PlayerPrefs.GetString("simSelectedRobot") + "\nHas it been moved or deleted?)");
return;
}
reset = FieldDataHandler.robotSpawn == new Vector3(99999, 99999, 99999);
if (RobotTypeManager.IsMixAndMatch && RobotTypeManager.HasManipulator)
{
Debug.Log(LoadManipulator(RobotTypeManager.ManipulatorPath) ? "Load manipulator success" : "Load manipulator failed");
}
}
else
{
awaitingReplay = true;
LoadReplay(selectedReplay);
}
//initializes the dynamic camera
DynamicCameraObject = GameObject.Find("Main Camera");
dynamicCamera = DynamicCameraObject.AddComponent <DynamicCamera>();
DynamicCamera.ControlEnabled = true;
sensorManager = GameObject.Find("SensorManager").GetComponent <SensorManager>();
sensorManagerGUI = StateMachine.gameObject.GetComponent <SensorManagerGUI>();
simUI = StateMachine.SceneGlobal.GetComponent <SimUI>();
robotCameraManager = GameObject.Find("RobotCameraList").GetComponent <RobotCameraManager>();
IsMetric = PlayerPrefs.GetString("Measure").Equals("Metric") ? true : false;
StateMachine.Link <MainState>(GameObject.Find("Main Camera").transform.GetChild(0).gameObject);
StateMachine.Link <MainState>(GameObject.Find("Main Camera").transform.GetChild(1).gameObject, false);
StateMachine.Link <ReplayState>(Auxiliary.FindGameObject("ReplayUI"));
StateMachine.Link <SaveReplayState>(Auxiliary.FindGameObject("SaveReplayUI"));
StateMachine.Link <GamepieceSpawnState>(Auxiliary.FindGameObject("ResetGamepieceSpawnpointUI"));
StateMachine.Link <DefineNodeState>(Auxiliary.FindGameObject("DefineNodeUI"));
StateMachine.Link <GoalState>(Auxiliary.FindGameObject("GoalStateUI"));
StateMachine.Link <SensorSpawnState>(Auxiliary.FindGameObject("ResetSensorSpawnpointUI"));
StateMachine.Link <DefineSensorAttachmentState>(Auxiliary.FindGameObject("DefineSensorAttachmentUI"));
string defaultDirectory = (Environment.GetFolderPath(Environment.SpecialFolder.ApplicationData) + @"Autodesk\Synthesis\Emulator");
string directoryPath = "";
if (Directory.Exists(defaultDirectory))
{
directoryPath = defaultDirectory;
isEmulationDownloaded = 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>
/// 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);
}
/// <summary>
/// Called after Awake() when the script instance is enabled.
/// Initializes variables then loads the field and robot as well as setting up replay features.
/// </summary>
public override void Start()
{
AppModel.ClearError();
//getting bullet physics information
physicsWorld = BPhysicsWorld.Get();
((DynamicsWorld)physicsWorld.world).SetInternalTickCallback(BPhysicsTickListener.Instance.PhysicsTick);
lastFrameCount = physicsWorld.frameCount;
//setting up raycast robot tick callback
BPhysicsTickListener.Instance.OnTick -= BRobotManager.Instance.UpdateRaycastRobots;
BPhysicsTickListener.Instance.OnTick += BRobotManager.Instance.UpdateRaycastRobots;
//setting up replay
CollisionTracker = new CollisionTracker(this);
//starts a new instance of unity packet which receives packets from the driver station
unityPacket = new UnityPacket();
unityPacket.Start();
//loads all the controls
Controls.Load();
//If a replay has been selected, load the replay. Otherwise, load the field and robot.
string selectedReplay = PlayerPrefs.GetString("simSelectedReplay");
SpawnedRobots = new List <Robot>();
if (string.IsNullOrEmpty(selectedReplay))
{
Tracking = true;
if (!LoadField(PlayerPrefs.GetString("simSelectedField")))
{
AppModel.ErrorToMenu("Could not load field: " + PlayerPrefs.GetString("simSelectedField") + "\nHas it been moved or deleted?)");
return;
}
if (!LoadRobot(PlayerPrefs.GetString("simSelectedRobot"), RobotTypeManager.IsMixAndMatch))
{
AppModel.ErrorToMenu("Could not load robot: " + PlayerPrefs.GetString("simSelectedRobot") + "\nHas it been moved or deleted?)");
return;
}
if (RobotTypeManager.IsMixAndMatch && RobotTypeManager.HasManipulator)
{
Debug.Log(LoadManipulator(RobotTypeManager.ManipulatorPath) ? "Load manipulator success" : "Load manipulator failed");
}
}
else
{
awaitingReplay = true;
LoadReplay(selectedReplay);
}
//initializes the dynamic camera
DynamicCameraObject = GameObject.Find("Main Camera");
dynamicCamera = DynamicCameraObject.AddComponent <DynamicCamera>();
DynamicCamera.MovingEnabled = true;
sensorManager = GameObject.Find("SensorManager").GetComponent <SensorManager>();
sensorManagerGUI = GameObject.Find("StateMachine").GetComponent <SensorManagerGUI>();
robotCameraManager = GameObject.Find("RobotCameraList").GetComponent <RobotCameraManager>();
IsMetric = PlayerPrefs.GetString("Measure").Equals("Metric") ? true : false;
StateMachine.Instance.Link <MainState>(GameObject.Find("Main Camera").transform.GetChild(0).gameObject);
StateMachine.Instance.Link <ReplayState>(Resources.FindObjectsOfTypeAll <GameObject>().First(x => x.name.Equals("ReplayUI")));
StateMachine.Instance.Link <SaveReplayState>(Resources.FindObjectsOfTypeAll <GameObject>().First(x => x.name.Equals("SaveReplayUI")));
}
// Update is called once per frame
public override void Update()
{
Ray ray = UnityEngine.Camera.main.ScreenPointToRay(UnityEngine.Input.mousePosition);
BulletSharp.Math.Vector3 start = ray.origin.ToBullet();
BulletSharp.Math.Vector3 end = ray.GetPoint(200).ToBullet();
//Creates a callback result that will be updated if we do a ray test with it
ClosestRayResultCallback rayResult = new ClosestRayResultCallback(ref start, ref end);
//Retrieves the bullet physics world and does a ray test with the given coordinates and updates the callback object
BPhysicsWorld world = BPhysicsWorld.Get();
world.world.RayTest(start, end, rayResult);
//If there is a collision object and it is dynamic and not a robot part, change the gamepiece to that
if (rayResult.CollisionObject != null)
{
GameObject collisionObject = (rayResult.CollisionObject.UserObject as BRigidBody).gameObject;
if (rayResult.CollisionObject.CollisionFlags == BulletSharp.CollisionFlags.StaticObject)
{
RevertNodeColors(hoveredNode, hoveredColors);
}
else if (collisionObject == null)
{
RevertNodeColors(hoveredNode, hoveredColors);
}
else if (collisionObject.transform.parent == StateMachine.SceneGlobal.FindState <MainState>().ActiveRobot.transform)
{
if (hoveredNode != collisionObject)
{
RevertNodeColors(hoveredNode, hoveredColors);
}
hoveredNode = collisionObject;
ChangeNodeColors(hoveredNode, hoverColor, hoveredColors);
if (UnityEngine.Input.GetMouseButtonUp(0))
{
if (camera)
{
robotCameraManager.CurrentCamera.transform.parent = hoveredNode.transform;
}
else
{
currentSensor.gameObject.transform.parent = hoveredNode.transform;
}
ReturnToMainState();
}
}
else
{
RevertNodeColors(hoveredNode, hoveredColors);
}
}
if (UnityEngine.Input.GetKeyDown(KeyCode.Escape))
{
ReturnToMainState();
}
if (EventSystem.current.currentSelectedGameObject == highlightButton.gameObject && UnityEngine.Input.GetMouseButton(0))
{
HighlightNode();
}
else
{
RevertHighlight();
}
//if (highlightTimer > 0) highlightTimer--;
//else if (highlightTimer == 0) RevertHighlight();
}
public int AddLinkToMultiBody(BMultiBody mb, int currentLinkIndex, int parentIndex, Transform parent)
{
if (isLinked)
{
Debug.LogErrorFormat("Cannot add link {0} to multibody {1} bacause it is already linked", name, mb.name);
return(0);
}
BCollisionShape collisionShape = GetComponent <BCollisionShape>();
if (collisionShape == null)
{
throw new MissingComponentException("Could not find " + typeof(BCollisionShape).Name + " component on BodyLink " + name);
}
multiBody = mb;
linkId = currentLinkIndex;
parentLinkId = parentIndex;
parentTransform = parent;
CollisionShape shape = collisionShape.GetCollisionShape();
if (shape == null)
{
throw new MissingComponentException("Could not get collision shape from " + collisionShape.GetType().Name + " shape component on BodyLink " + name);
}
BulletSharp.Math.Vector3 linkInertia;
shape.CalculateLocalInertia(Mass, out linkInertia);
if (BPhysicsWorld.Get().debugType >= BulletUnity.Debugging.BDebug.DebugType.Debug)
{
Debug.LogFormat(this, "Adding link {0} : {1} to parent {2} of multibody {3}", currentLinkIndex, name, parentIndex, mb.name);
}
SetupLink(linkInertia);
linkCollider = new MultiBodyLinkCollider(mb.MultiBody, currentLinkIndex);
linkCollider.CollisionShape = shape;
linkCollider.WorldTransform = transform.localToWorldMatrix.ToBullet();
linkCollider.CollisionFlags = collisionFlags;
linkCollider.Friction = Friction;
linkCollider.RollingFriction = RollingFriction;
linkCollider.Restitution = Restitution;
linkCollider.UserObject = this;
BPhysicsWorld.Get().world.AddCollisionObject(linkCollider, groupsIBelongTo, collisionMask);
m_collisionObject = linkCollider;
BulletMultiBodyLinkColliderProxy proxy = gameObject.GetComponent <BulletMultiBodyLinkColliderProxy>();
if (proxy == null)
{
proxy = gameObject.AddComponent <BulletMultiBodyLinkColliderProxy>();
}
mb.MultiBody.GetLink(currentLinkIndex).Collider = linkCollider;
proxy.target = linkCollider;
isLinked = true;
foreach (BMultiBodyConstraint mbc in Constraints)
{
mbc.AddConstraintToMultiBody(MultiBody, LinkId);
}
int addedLinks = 1;
for (int i = 0; i < Links.Count; ++i)
{
addedLinks += Links[i].AddLinkToMultiBody(mb, i + currentLinkIndex + 1, currentLinkIndex, transform);
}
return(addedLinks);
}
public override void BOnCollisionExit(CollisionObject other)
{
Debug.Log("On Collision Exit " + BPhysicsWorld.Get().frameCount);
}
public override void BOnCollisionStay(CollisionObject other, PersistentManifoldList manifoldList)
{
Debug.Log("On Collision Stay " + BPhysicsWorld.Get().frameCount);
}