public void ApplyForce()
{
Ray ray = UnityEngine.Camera.main.ScreenPointToRay(UnityEngine.Input.mousePosition);
float impulse;
float.TryParse(GameObject.Find("ImpulseInputField").GetComponent <InputField>().text, out impulse);
Vector3 a = ray.origin + (ray.direction * 1000);
BulletSharp.Math.Vector3 from = new BulletSharp.Math.Vector3(ray.origin.x, ray.origin.y, ray.origin.z),
to = new BulletSharp.Math.Vector3(a.x, a.y, a.z);
ClosestRayResultCallback callback = new ClosestRayResultCallback(ref from, ref to);
BPhysicsWorld world = BPhysicsWorld.Get();
world.world.RayTest(from, to, callback);
BulletSharp.Math.Vector3 point = callback.HitNormalWorld;
BRigidBody part = (BRigidBody)callback.CollisionObject.UserObject;
foreach (BRigidBody br in GameObject.Find("Robot").GetComponentsInChildren <BRigidBody>())
{
if (part == br)
{
Vector3 closestPoint = br.GetComponent <MeshRenderer>().bounds.ClosestPoint(point.ToUnity());
Ray normalRay = new Ray(point.ToUnity(), closestPoint - point.ToUnity());
part.AddImpulseAtPosition(ray.direction.normalized * impulse, point.ToUnity());
// part.AddImpulseAtPosition(normalRay.direction.normalized * impulse, normalRay.origin);
}
}
}
/// <summary>
/// Adds a wheel to the BRaycastVehicle from the given information.
/// </summary>
/// <param name="connectionPoint"></param>
/// <param name="axle"></param>
/// <param name="suspensionRestLength"></param>
/// <param name="radius"></param>
/// <returns></returns>
public int AddWheel(WheelType wheelType, BulletSharp.Math.Vector3 connectionPoint, BulletSharp.Math.Vector3 axle, float suspensionRestLength, float radius)
{
float slidingFriction = DefaultSlidingFriction;
switch (wheelType)
{
case WheelType.MECANUM:
slidingFriction = 0.1f;
axle = (Quaternion.AngleAxis((connectionPoint.X > 0 && connectionPoint.Z > 0) || (connectionPoint.X < 0 && connectionPoint.Z < 0) ? -45 : 45,
Vector3.up) * axle.ToUnity()).ToBullet();
break;
case WheelType.OMNI:
slidingFriction = 0.1f;
break;
}
RobotWheelInfo wheel = RaycastRobot.AddWheel(connectionPoint,
-BulletSharp.Math.Vector3.UnitY, axle, suspensionRestLength,
radius, defaultVehicleTuning, false);
wheel.RollInfluence = RollInfluence;
wheel.SlidingFriction = slidingFriction;
return(RaycastRobot.NumWheels - 1);
}
//Step #2
public override float ReturnOutput()
{
//setting shortest distance of a collider to the maxRange, then if any colliders are closer to the sensor,
//their distanceToCollider value becomes the new shortest distance
float shortestDistance = maxRange;
//Raycasting begins
Ray ray = new Ray(gameObject.transform.position, transform.forward);
BulletSharp.Math.Vector3 fromUltra = ray.origin.ToBullet();
BulletSharp.Math.Vector3 toCollider = ray.GetPoint(maxRange).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);
//Gets the position of all hit points of the ray test
List <BulletSharp.Math.Vector3> colliderPositions = raysCallback.HitPointWorld;
BulletSharp.Math.Vector3 colliderPosition = BulletSharp.Math.Vector3.Zero;
float distanceToCollider = maxRange;
//Loop through all hit points and get the shortest distance, exclude the origin since it is also counted as a hit point
foreach (BulletSharp.Math.Vector3 pos in colliderPositions)
{
if ((pos - fromUltra).Length < distanceToCollider && !pos.Equals(BulletSharp.Math.Vector3.Zero))
{
distanceToCollider = (pos - fromUltra).Length;
colliderPosition = pos;
}
}
Debug.DrawLine(fromUltra.ToUnity(), colliderPosition.ToUnity(), Color.green, 5f);
if (distanceToCollider < shortestDistance)
{
shortestDistance = distanceToCollider;
}
//Will need when data sent to emulator
//if (shortestDistance == maxRange)
//{
// //read out to user that nothing within range was detected by ultrasonic sensor;
// Debug.Log("False");
//}
//else
//{
// //read out to user that first object detected was 'shortestDistance' away;
// Debug.Log("True");
//}
return(shortestDistance);
}
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;
float distanceToCollider = 0;
//Set the initial distance as the distance between emitter and receiver
if (main != null && main.IsMetric)
{
distanceToCollider = sensorOffset;
}
else
{
distanceToCollider = Auxiliary.ToFeet(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
state = "Broken";
return(1);
}
else
{
//Nothing in between
state = "Unbroken";
return(0);
}
}
//Step #2
public override float ReturnOutput()
{
//Raycasting begins
Ray ray = new Ray(gameObject.transform.position, transform.forward);
BulletSharp.Math.Vector3 fromUltra = ray.origin.ToBullet();
BulletSharp.Math.Vector3 toCollider = ray.GetPoint(MaxRange).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);
//Gets the position of all hit points of the ray test
List <BulletSharp.Math.Vector3> colliderPositions = raysCallback.HitPointWorld;
BulletSharp.Math.Vector3 colliderPosition = BulletSharp.Math.Vector3.Zero;
float distanceToCollider = MaxRange;
if (main != null && main.IsMetric)
{
distanceToCollider = MaxRange;
foreach (BulletSharp.Math.Vector3 pos in colliderPositions)
{
if ((pos - fromUltra).Length < MaxRange && !pos.Equals(BulletSharp.Math.Vector3.Zero))
{
distanceToCollider = (pos - fromUltra).Length;
colliderPosition = pos;
}
}
}
else
{
distanceToCollider = Auxiliary.ToFeet(MaxRange);
foreach (BulletSharp.Math.Vector3 pos in colliderPositions)
{
if (Auxiliary.ToFeet((pos - fromUltra).Length) < distanceToCollider && !pos.Equals(BulletSharp.Math.Vector3.Zero))
{
distanceToCollider = Auxiliary.ToFeet((pos - fromUltra).Length);
colliderPosition = pos;
}
}
}
//Draw a line to view the ray action
//When the ray links to the middle of the field, it means the sensor is out of range
Debug.DrawLine(fromUltra.ToUnity(), colliderPosition.ToUnity(), Color.green, 5f);
return(distanceToCollider);
}
//Step #2
public override float ReturnOutput()
{
//Raycasting begins
Ray ray = new Ray(gameObject.transform.position, transform.forward);
BulletSharp.Math.Vector3 fromUltra = ray.origin.ToBullet();
BulletSharp.Math.Vector3 toCollider = ray.GetPoint(MaxRange).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);
//Gets the position of all hit points of the ray test
List <BulletSharp.Math.Vector3> colliderPositions = raysCallback.HitPointWorld;
BulletSharp.Math.Vector3 colliderPosition = BulletSharp.Math.Vector3.Zero;
float distanceToCollider = MaxRange;
//Loop through all hit points and get the shortest distance, exclude the origin since it is also counted as a hit point
foreach (BulletSharp.Math.Vector3 pos in colliderPositions)
{
if ((pos - fromUltra).Length <= MaxRange && (pos - fromUltra).Length < distanceToCollider && !pos.Equals(BulletSharp.Math.Vector3.Zero))
{
distanceToCollider = (pos - fromUltra).Length;
colliderPosition = pos;
}
}
//Draw a line to view the ray action
//When the ray links to the middle of the field, it means the sensor is out of range
Debug.DrawLine(fromUltra.ToUnity(), colliderPosition.ToUnity(), Color.green, 5f);
//setting shortest distance of a collider to the maxRange, then if any colliders are closer to the sensor,
//their distanceToCollider value becomes the new shortest distance
float shortestDistance = MaxRange;
if (!isMetric)
{
distanceToCollider = AuxFunctions.ToFeet(distanceToCollider);
}
//A check that might be useful in the future if use a bundle of rays instead of a single ray
if (distanceToCollider < shortestDistance)
{
shortestDistance = distanceToCollider;
}
return(shortestDistance);
}
void FixedUpdate()
{
BulletSharp.Math.Vector3 from = transform.position.ToBullet();
List <bool> hits = new List <bool>()
{
false, false, false, false, false
};
//float theta = angle * Mathf.PI;
//float sin = Mathf.Sin(theta);
//float cos = Mathf.Cos(theta);
//float tan = Mathf.Tan(theta);
var endPos = transform.position + (Vector3.down * (capsule.Height + heightExtension));
for (int whiskerNum = 0; whiskerNum < 5; whiskerNum++)
{
BulletSharp.Math.Vector3 to;
switch (whiskerNum)
{
case 0:
to = endPos.ToBullet();
break;
case 1:
to = (endPos + (Vector3.forward * whiskerDist)).ToBullet();
break;
case 2:
to = (endPos + (Vector3.back * whiskerDist)).ToBullet();
break;
case 3:
to = (endPos + (Vector3.left * whiskerDist)).ToBullet();
break;
case 4:
to = (endPos + (Vector3.right * whiskerDist)).ToBullet();
break;
default:
to = endPos.ToBullet();
break;
}
ClosestRayResultCallback callback = new ClosestRayResultCallback(ref from, ref to);
BPhysicsWorld.Get().world.RayTest(from, to, callback);
hits[whiskerNum] = callback.HasHit;
Debug.DrawLine(from.ToUnity(), to.ToUnity(), callback.HasHit ? Color.green : Color.red);
}
isGrounded = hits.Contains(true);
}
void RpcUpdateTransforms(float[] transforms)
{
if (isServer || rigidBodies == null)
{
return;
}
BulletSharp.Math.Matrix[] bmTransforms = new BulletSharp.Math.Matrix[rigidBodies.Length];
// TODO: Combine these loops.
for (int i = 0; i < bmTransforms.Length; i++)
{
float[] rawTransform = new float[7];
for (int j = 0; j < rawTransform.Length; j++)
{
rawTransform[j] = transforms[i * 13 + j];
}
bmTransforms[i] = BulletExtensions.DeserializeTransform(rawTransform);
rigidBodies[i].GetCollisionObject().Activate();
BulletSharp.Math.Matrix rbTransform = rigidBodies[i].GetCollisionObject().WorldTransform;
}
for (int i = 0; i < bmTransforms.Length; i++)
{
float[] rawLinearVelocity = new float[3];
for (int j = 0; j < rawLinearVelocity.Length; j++)
{
rawLinearVelocity[j] = transforms[i * 13 + 7 + j];
}
float[] rawAngularVelocity = new float[3];
for (int j = 0; j < rawAngularVelocity.Length; j++)
{
rawAngularVelocity[j] = transforms[i * 13 + 7 + rawLinearVelocity.Length + j];
}
BulletSharp.Math.Vector3 linearVelocity = new BulletSharp.Math.Vector3(rawLinearVelocity);
BulletSharp.Math.Vector3 angularVelocity = new BulletSharp.Math.Vector3(rawAngularVelocity);
networkMeshes[i].TargetLinearVelocity = linearVelocity.ToUnity();
RigidBody rbCo = (RigidBody)rigidBodies[i].GetCollisionObject();
rbCo.WorldTransform = bmTransforms[i];
rbCo.LinearVelocity = linearVelocity;
rbCo.AngularVelocity = angularVelocity;
}
}
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);
}
}
}
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
}
}
void RpcUpdateTransform(float[] transform)
{
if (isServer || rigidBody == null)
{
return;
}
BulletSharp.Math.Matrix bmTransform = BulletExtensions.DeserializeTransform(transform.Take(7).ToArray());
BulletSharp.Math.Matrix rbTransform = rigidBody.WorldTransform;
BulletSharp.Math.Vector3 linearVelocity = new BulletSharp.Math.Vector3(transform.Skip(7).Take(3).ToArray());
BulletSharp.Math.Vector3 angularVelocity = new BulletSharp.Math.Vector3(transform.Skip(10).Take(3).ToArray());
if (networkMesh != null)
{
networkMesh.TargetLinearVelocity = linearVelocity.ToUnity();
}
rigidBody.WorldTransform = bmTransform;
rigidBody.LinearVelocity = linearVelocity;
rigidBody.AngularVelocity = angularVelocity;
}
public void CreateUnityMultiBodyLinkColliderProxy(MultiBodyLinkCollider body)
{
GameObject cube = Instantiate <GameObject>(cubePrefab);
CollisionShape cs = body.CollisionShape;
if (cs is BoxShape)
{
BoxShape bxcs = cs as BoxShape;
BulletSharp.Math.Vector3 s = bxcs.HalfExtentsWithMargin;
MeshRenderer mr = cube.GetComponentInChildren <MeshRenderer>();
mr.transform.localScale = s.ToUnity() * 2f;
Matrix4x4 m = body.WorldTransform.ToUnity();
cube.transform.position = BSExtensionMethods2.ExtractTranslationFromMatrix(ref m);
cube.transform.rotation = BSExtensionMethods2.ExtractRotationFromMatrix(ref m);
cube.transform.localScale = BSExtensionMethods2.ExtractScaleFromMatrix(ref m);
Destroy(cube.GetComponent <BulletRigidBodyProxy>());
BulletMultiBodyLinkColliderProxy cp = cube.AddComponent <BulletMultiBodyLinkColliderProxy>();
cp.target = body;
}
else
{
Debug.LogError("Not implemented");
}
}
/// <summary>
/// Adds a wheel to the BRaycastVehicle from the given information.
/// </summary>
/// <param name="connectionPoint"></param>
/// <param name="axle"></param>
/// <param name="suspensionRestLength"></param>
/// <param name="radius"></param>
/// <returns></returns>
public int AddWheel(WheelType wheelType, BulletSharp.Math.Vector3 connectionPoint, BulletSharp.Math.Vector3 axle, float suspensionRestLength, float radius)
{
switch (wheelType)
{
case WheelType.MECANUM:
RaycastRobot.SlidingFriction = 0.1f;
axle = (Quaternion.AngleAxis((connectionPoint.X > 0 && connectionPoint.Z > 0) || (connectionPoint.X < 0 && connectionPoint.Z < 0) ? -45 : 45,
Vector3.up) * axle.ToUnity()).ToBullet();
break;
case WheelType.OMNI:
RaycastRobot.SlidingFriction = 0.1f;
break;
}
WheelInfo w = RaycastRobot.AddWheel(connectionPoint,
-BulletSharp.Math.Vector3.UnitY, axle, suspensionRestLength,
radius, vehicleTuning, false);
w.RollInfluence = 0.25f;
w.Brake = (RollingFriction / radius) * RaycastRobot.OverrideMass * BRaycastWheel.MassTorqueScalar;
return(RaycastRobot.NumWheels - 1);
}
