/***********************************************************************************************
******************** HIT TEST FUNCTIONS ******************************
***********************************************************************************************/
/// <summary>
/// Static functions for checking collisions or 'hit' with the real world.
/// In each function, "countinvalidascollision" specifies if off-screen pixels or missing depth values should count as collision.
/// "realworldthickness" specifies how far back a point needs to be behind the real world before it's not considered a collision.
/// </summary>
/// <summary>
/// Checks an individual point in world space to see if it's occluded by the real world.
/// </summary>
/// <param name="point">3D point in the world that belongs to a virtual object</param>
/// <param name="camera">camera (usually left camera)</param>
/// <returns>True if the test represents a valid hit test.</returns>
public static bool HitTestAtPoint(Camera camera, Vector3 point, bool countinvalidascollision = false, float realworldthickness = Mathf.Infinity)
{
//Transform the point into screen space
Vector3 screenpoint = camera.WorldToScreenPoint(point);
//Make sure it's within our view frustrum (except for clipping planes)
if (!CheckScreenView(point, camera))
{
return(countinvalidascollision);
}
//Compare distance in _virtual camera to corresponding point in distance map.
float realdistance;
ZEDSupportFunctions.GetEuclideanDistanceAtPixel(new Vector2(screenpoint.x, screenpoint.y), out realdistance);
//If we pass bad parameters, or we don't have an accurate reading on the depth, we can't test.
if (realdistance <= 0f)
{
return(countinvalidascollision); //We can't read the depth from that pixel.
}
///Detection is the space
if (realdistance <= Vector3.Distance(point, camera.transform.position) && Vector3.Distance(point, camera.transform.position) - realdistance <= realworldthickness)
{
return(true); //The real pixel is closer or at the same depth as the virtual point. That's a collision.
}
else
{
return(false); //It's behind the virtual point.
}
}
/// <summary>
/// Tests the depth of both the real and virtual in the center of the screen, and returns the world position of the closest one.
/// </summary>
/// <param name="crosshairpoint">Where the crosshair should be rendered.</param>
/// <param name="collisionnormal">The normal vector of the surface aimed at, for rotating the crosshair accordingly if desired.</param>
/// <returns>False if there is no valid object, real or virtual, on which to place the crosshair. </returns>
private bool FindCrosshairPosition(out Vector3 crosshairpoint, out Vector3 collisionnormal)
{
//Find the distance to the real world. The bool will be false if there is an error reading the depth at the center of the screen.
Vector3 realpoint;
bool foundrealdistance = ZEDSupportFunctions.HitTestOnRay(leftcamera, laserPointerBeadHolder.transform.position, laserPointerBeadHolder.transform.rotation, 5f, 0.01f, out realpoint);
float realdistance = Vector3.Distance(laserPointerBeadHolder.transform.position, realpoint);
//Find the distance to the virtual. The bool will be false if there are no colliders ahead of you.
RaycastHit hitinfo;
bool foundvirtualdistance = Physics.Raycast(laserPointerBeadHolder.transform.position, laserPointerBeadHolder.transform.rotation * Vector3.forward, out hitinfo);
//If we didn't find either, return false so the laser and bead can be disabled.
if (!foundrealdistance && !foundvirtualdistance)
{
crosshairpoint = Vector3.zero;
collisionnormal = Vector3.zero;
return(false);
}
//Decide if we use the real or virtual distance
if (!foundvirtualdistance || realdistance < hitinfo.distance)
{
//The real world is closer. Give the position of the real world pixel and return true.
crosshairpoint = realpoint;
ZEDSupportFunctions.GetNormalAtWorldLocation(realpoint, sl.REFERENCE_FRAME.WORLD, leftcamera, out collisionnormal);
return(true);
}
else
{
//The virtual world is closer, or they're tied. Return the world posiiton where the raycast hit the virtual collider.
crosshairpoint = hitinfo.point;
collisionnormal = hitinfo.normal;
return(true);
}
}
/// <summary>
/// Handles movements and collisions on a constant basis.
/// </summary>
void FixedUpdate()
{
//Calculate where the object should move this frame
Vector3 newpos = transform.position + transform.rotation * Vector3.forward * (speed * Time.deltaTime);
//Collisions with the real World. As the object moves, collisions checks are made each frame at the next position.
Vector3 collisionpoint;
Vector3 collisionnormal;
//First, test the primary ZED. Collisions will look the most accurate if calculated from this one.
bool primaryhit = ZEDSupportFunctions.HitTestOnRay(zedManager.zedCamera, cam, newpos, transform.rotation, Vector3.Distance(transform.position, newpos),
distanceBetweenRayChecks, out collisionpoint, false, realWorldThickness);
if (primaryhit)
{
//Call the function to resolve the impact. This allows us to easily override what happens on collisions with classes that inherit this one.
ZEDSupportFunctions.GetNormalAtWorldLocation(zedManager.zedCamera, collisionpoint, sl.REFERENCE_FRAME.WORLD, cam, out collisionnormal);
OnHitRealWorld(collisionpoint, collisionnormal);
}
if (!primaryhit && testCollisionsUsingAllZEDs) //If set to true, test the rest of the ZEDs as well.
{
foreach (ZEDManager manager in ZEDManager.GetInstances())
{
if (manager == zedManager)
{
continue; //If it's the primary ZED, skip as we've already tested that one.
}
if (ZEDSupportFunctions.HitTestOnRay(manager.zedCamera, manager.GetMainCamera(), newpos, transform.rotation, Vector3.Distance(transform.position, newpos), distanceBetweenRayChecks, out collisionpoint, false, realWorldThickness))
{
//Call the function to resolve the impact. This allows us to easily override what happens on collisions with classes that inherit this one.
ZEDSupportFunctions.GetNormalAtWorldLocation(manager.zedCamera, collisionpoint, sl.REFERENCE_FRAME.WORLD, manager.GetMainCamera(), out collisionnormal);
OnHitRealWorld(collisionpoint, collisionnormal);
break; //No need to test the rest of the ZEDs.
}
}
}
//Collisions with virtual objects
//Cast a ray to check collisions between here and the intended move point for virtual objects.
RaycastHit hitinfo;
if (Physics.Raycast(transform.position, newpos - transform.position, out hitinfo, Vector3.Distance(transform.position, newpos)))
{
//Call the function to resolve the impact. This allows us to easily override what happens on collisions with classes that inherit this one.
OnHitVirtualWorld(hitinfo);
}
//Move it to this new place
transform.position = newpos;
//Tick down its lifespan and check if we should destroy it.
lifespan -= Time.deltaTime;
if (lifespan <= 0f)
{
Destroy(gameObject);
}
}
/// <summary>
/// This function is called every fixed framerate frame
/// Here we take care of enabling & disabling the laser pointer.
/// </summary>
private void FixedUpdate()
{
//If we have been moved by the baseball bat
if (IsMoving)
{
//Look for our next position based on our current velocity.
Vector3 predictedPos = centerpoint.position + (rb.velocity * (Time.deltaTime * 2.5f));
transform.rotation = Quaternion.LookRotation(rb.velocity.normalized);
//Collision check with the real world at that next position.
if (ZEDSupportFunctions.HitTestAtPoint(leftcamera, predictedPos))
{
//We hit something, but is it a flat surface?
if (planeManager.DetectPlaneAtHit(leftcamera.WorldToScreenPoint(predictedPos)))
{
bunnyspawner.SpawnUI(predictedPos);
IsMoving = false;
}
else//If not, bounce off of it but still show the flag.
{
IsMoving = false; //Not moving anymore, so update our state.
bunnyspawner.SpawnUI(predictedPos); //Start spawning the UI on our current location.
rb.velocity = Vector3.Reflect(rb.velocity / 2, transform.forward); //Bounce off the surface we hit
}
}
}
}
/// <summary>
/// Gets the minimum distance to plane boundaries of a given 3D point (in world space)
/// </summary>
/// <returns>The minimum distance to boundaries.</returns>
/// <param name="worldPosition">World position.</param>
public float getMinimumDistanceToBoundaries(Vector3 worldPosition, out Vector3 minimumBoundsPosition)
{
var leftCamera = ZEDManager.Instance.GetLeftCameraTransform().gameObject.GetComponent <Camera>();
var minimal_distance = ZEDSupportFunctions.DistancePointLine(worldPosition,
leftCamera.transform.TransformPoint(planeData.Bounds[0]),
leftCamera.transform.TransformPoint(planeData.Bounds[1]));
var BestFoundPoint = new Vector3(0.0f, 0.0f, 0.0f);
if (planeData.BoundsSize > 2)
{
for (var i = 1; i < planeData.BoundsSize - 1; i++)
{
var currentDistance = ZEDSupportFunctions.DistancePointLine(worldPosition,
leftCamera.transform.TransformPoint(planeData.Bounds[i]),
leftCamera.transform.TransformPoint(planeData.Bounds[i + 1]));
if (currentDistance < minimal_distance)
{
minimal_distance = currentDistance;
BestFoundPoint = ZEDSupportFunctions.ProjectPointLine(worldPosition,
leftCamera.transform.TransformPoint(planeData.Bounds[i]),
leftCamera.transform.TransformPoint(planeData.Bounds[i + 1]));
}
}
}
minimumBoundsPosition = BestFoundPoint;
return(minimal_distance);
}
/// <summary>
/// This function is called every fixed framerate frame
/// Here we take care of enabling & disabling the laser pointer.
/// </summary>
private void FixedUpdate()
{
//If we have been moved by the baseball bat
if (_moving)
{
//Look for our next position based on our current velocity.
Vector3 predictedPos = _centerPoint.position + (_rb.velocity * (Time.deltaTime * 2.5f));
transform.rotation = Quaternion.LookRotation(_rb.velocity.normalized);
//Collision check with the real world at that next position.
if (ZEDSupportFunctions.HitTestAtPoint(_leftCamera, predictedPos))
{
//We hit something, but is it a flat surface?
if (_zedPlane.DetectPlaneAtHit(_leftCamera.WorldToScreenPoint(predictedPos)))
{
_mySpawner.SpawnUI(predictedPos);
_moving = false;
}
else//If not freeze on hit.
{
//_rb.isKinematic = true; //Freeze the object at the current position.
_moving = false; //Not moving anymore, so update our state.
_mySpawner.SpawnUI(predictedPos); //Start spawning the UI on our current location.
_rb.velocity = Vector3.Reflect(_rb.velocity / 2, transform.forward);
}
}
}
}
/// <summary>
/// Checks nearby for valid places for the drone to move.
/// Valid places must be in front of the player, and not intersect any objects within a reasonable tolerance.
/// Use radiusCheckRate and percentageThreshold to tweak what counts as a valid location.
/// </summary>
/// <param name="newpos"></param>
/// <returns></returns>
private bool FindNewMovePosition(out Vector3 newpos)
{
//We can't move if the ZED isn't initialized.
if (!zedManager.IsZEDReady)
{
newpos = transform.position;
return(false);
}
Vector3 randomPosition;
// Look Around For a New Position
//If the Drone is on the screen, search around a smaller radius.
//Note that we only check the primary ZEDManager because we only want the drone to spawn in front of the player anyway.
if (ZEDSupportFunctions.CheckScreenView(transform.position, zedManager.GetLeftCamera()))
{
randomPosition = UnityEngine.Random.onUnitSphere * UnityEngine.Random.Range(2f, 3f) + transform.position;
}
else //if the drone is outside, look around a bigger radius to find a position which is inside the screen.
{
randomPosition = UnityEngine.Random.onUnitSphere * UnityEngine.Random.Range(4f, 5f) + transform.position;
}
// Look For Any Collisions Through The ZED
bool hit = ZEDSupportFunctions.HitTestAtPoint(zedManager.zedCamera, zedManager.GetLeftCamera(), randomPosition);
if (!hit)
{
newpos = transform.position;
return(false);
}
//If we spawn the drone at that world point, it'll spawn inside a wall. Bring it closer by a distance of ClearRadius.
Quaternion directiontoDrone = Quaternion.LookRotation(zedManager.GetLeftCameraTransform().position - randomPosition, Vector3.up);
Vector3 newPosition = randomPosition + directiontoDrone * Vector3.forward * ClearRadius;
//Check the new position isn't too close from the camera.
float dist = Vector3.Distance(zedManager.GetLeftCamera().transform.position, randomPosition);
if (dist < 1f)
{
newpos = transform.position;
return(false);
}
//Also check nearby points in a sphere of radius to make sure the whole drone has a clear space.
if (ZEDSupportFunctions.HitTestOnSphere(zedManager.zedCamera, zedManager.GetLeftCamera(), newPosition, 1f, radiusCheckRate, percentageThreshold))
{
newpos = transform.position;
return(false);
}
//Return true if it's made it this far and out the location we chose.
newpos = newPosition;
return(true);
}
/// <summary>
/// Checks if another ArUcoDrone is in front of the drone and within range.
/// Will also check if there's a real object in the way, if checkRealWorldObstaclesBeforeShooting is true.
/// </summary>
/// <returns>True if there's a valid target in front of the drone.</returns>
private bool IsAimingAtTarget()
{
//First make sure there's a valid virtual target in front of the drone.
Ray ray = new Ray(shootAnchorObject.position, shootAnchorObject.rotation * Vector3.forward);
RaycastHit[] hits = Physics.RaycastAll(ray, shootRangeMeters);
bool foundvirtualtarget = false;
float nearestdist = Mathf.Infinity;
foreach (RaycastHit hit in hits)
{
ArUcoDrone otherdrone = hit.transform.GetComponent <ArUcoDrone>();
if (otherdrone != null && otherdrone != this)
{
foundvirtualtarget = true;
if (hit.distance < nearestdist)
{
nearestdist = hit.distance;
}
}
}
if (!foundvirtualtarget)
{
return(false);
}
if (checkRealWorldObstaclesBeforeShooting) //Make sure there's not a real-world obstacle in the way of the target.
{
//If there is one, check to make sure there's not a real-world object in the way.
Vector3 collisionpoint; //Not used but required for HitTestOnRay function.
foreach (ZEDManager manager in ZEDManager.GetInstances())
{
bool hitreal = ZEDSupportFunctions.HitTestOnRay(manager.zedCamera, manager.GetLeftCamera(), shootAnchorObject.transform.position, shootAnchorObject.transform.rotation,
nearestdist, 0.01f, out collisionpoint, false, 0.1f);
if (hitreal)
{
return(false);
}
}
return(true);
}
else
{
return(true); //We're not checking against the real world, and we already found a virtual object, so fire.
}
}
// Raycast from screen space to world space
// The GetWorldPositionAtPixel function is innacurate when using ZED_Rig_Mono for some reason. May be a bug with the Unity plugin.
// Raycast from the camera in the direction of the world space position we determined from the provided screen space position
public RaycastHit raycastFromScreenSpace(Vector2 screenPosition, ZEDManager manager)
{
Camera leftcamera = manager.GetLeftCamera();
RaycastHit hit;
int raycastLength = 20;
Vector3 worldPos;
ZEDSupportFunctions.GetWorldPositionAtPixel(zedCamera, screenPosition, leftcamera, out worldPos);
Vector3 direction = (worldPos - leftcamera.transform.position).normalized;
// Debug.DrawRay(leftcamera.transform.position, direction * raycastLength, Color.green, 30f);
Physics.Raycast(leftcamera.transform.position, direction * raycastLength, out hit);
return(hit);
}
// Update is called once per frame
void Update()
{
if (!sl.ZEDCamera.GetInstance().IsCameraReady)
{
return;
}
if (Input.GetMouseButtonDown(0))
{
/// Mouse Input gives the screen pixel position
Vector2 ScreenPosition = Input.mousePosition;
Vector3 Normal;
Vector3 WorldPos;
// Get Normal and real world position defined by the pixel
ZEDSupportFunctions.GetNormalAtPixel(ScreenPosition, sl.REFERENCE_FRAME.WORLD, LeftCamera, out Normal);
ZEDSupportFunctions.GetWorldPositionAtPixel(ScreenPosition, LeftCamera, out WorldPos);
// To consider the location as a floor, we check that the normal is valid and is closely aligned with the gravity
bool validFloor = Normal.x != float.NaN && Vector3.Dot(Normal, Vector3.up) > 0.85f;
// If we've found a floor to place the bunny, then set its location and show it.
if (validFloor)
{
GameObject newbunny = Instantiate(Object);
newbunny.transform.localPosition = WorldPos;
newbunny.transform.LookAt(new Vector3(ZedManager.transform.position.x, newbunny.transform.position.y, ZedManager.transform.position.z), Vector3.up);
newbunny.SetActive(true);
}
else
{
if (Normal.x == float.NaN)
{
Debug.Log("cannot place object at this position. Normal vector not detected.");
}
if (Vector3.Dot(Normal, Vector3.up) <= 0.85f)
{
Debug.Log("cannot place object at this position. Normal vector angled too far from up: " + Mathf.Acos(Vector3.Dot(Normal, Vector3.up)) * Mathf.Rad2Deg + "°");
}
Object.SetActive(false);
}
}
}
/// <summary>
/// Tests the depth of the real world based on the pointer origin position and rotation.
/// Returns the world position if it collided with anything.
/// </summary>
/// <param name="pointerbeadpoint">The world space position where the pointer is pointing.</param>
/// <returns>True if a valid real world point was found.</returns>
bool FindPointerPosition(out Vector3 pointerbeadpoint)
{
//Find the distance to the real world. The bool will be false if there is an error reading the depth at the center of the screen.
Vector3 realpoint;
bool foundrealdistance = ZEDSupportFunctions.HitTestOnRay(leftcamera, rayOrigin.position, rayOrigin.rotation, 5.0f, 0.05f, out realpoint);
//If we didn't find, return false so the laser and bead can be disabled.
if (!foundrealdistance)
{
pointerbeadpoint = Vector3.zero;
return(false);
}
else //Output the world position of the collision.
{
pointerbeadpoint = realpoint;
return(true);
}
}
/// <summary>
/// Tests the depth of the real world based on the pointer origin position and rotation.
/// Returns the world position if it collided with anything.
/// </summary>
/// <param name="pointerbeadpoint">The world space position where the pointer is pointing.</param>
/// <returns>True if a valid real world point was found.</returns>
bool FindPointerPosition(out Vector3 pointerbeadpoint)
{
//Find the distance to the real world. The bool will be false if there is an error reading the depth at the center of the screen.
Vector3 realpoint;
foreach (ZEDManager manager in ZEDManager.GetInstances()) //Check all cameras, in case it's hitting something the main ZEDManager can't see.
{
if (ZEDSupportFunctions.HitTestOnRay(zedManager.zedCamera, leftcamera, rayOrigin.position, rayOrigin.rotation, 5.0f, 0.05f, out realpoint))
{
pointerbeadpoint = realpoint;
return(true); //No need to check the other cameras.
}
}
//No camera was able to see a collision.
pointerbeadpoint = Vector3.zero;
return(false);
}
// Update is called once per frame
void Update()
{
if (!zedManager.zedCamera.IsCameraReady)
{
return;
}
if (Input.GetMouseButtonDown(0)) //Checks for left click.
{
/// Mouse Input gives the screen pixel position
Vector2 ScreenPosition = Input.mousePosition;
//Get Normal and real world position defined by the pixel .
Vector3 Normal;
Vector3 WorldPos;
ZEDSupportFunctions.GetNormalAtPixel(zedManager.zedCamera, ScreenPosition, sl.REFERENCE_FRAME.WORLD, LeftCamera, out Normal);
ZEDSupportFunctions.GetWorldPositionAtPixel(zedManager.zedCamera, ScreenPosition, LeftCamera, out WorldPos);
//To consider the location as a flat surface, we check that the normal is valid and is closely aligned with gravity.
bool validFloor = Normal.x != float.NaN && Vector3.Dot(Normal, Vector3.up) > 0.85f;
//If we've found a floor to place the object, spawn a copy of the prefab.
if (validFloor)
{
GameObject newgo = Instantiate(ObjectToPlace);
newgo.transform.localPosition = WorldPos;
newgo.transform.LookAt(new Vector3(zedManager.transform.position.x, newgo.transform.position.y, zedManager.transform.position.z), Vector3.up);
}
else
{
if (Normal.x == float.NaN)
{
Debug.Log("Cannot place object at this position. Normal vector not detected.");
}
if (Vector3.Dot(Normal, Vector3.up) <= 0.85f)
{
Debug.Log("Cannot place object at this position. Normal vector angled too far from up: " + Mathf.Acos(Vector3.Dot(Normal, Vector3.up)) * Mathf.Rad2Deg + "°");
}
}
}
}
void updateSkeleton()
{
float width = 0.025f;
for (int j = 0; j < spheres.Length; j++)
{
if (ZEDSupportFunctions.IsVector3NaN(joint[sphereList[j]]))
{
spheres[j].transform.position = Vector3.zero;
spheres[j].SetActive(false);
}
else
{
spheres[j].transform.position = joint[sphereList[j]];
spheres[j].SetActive(true);
}
}
for (int i = 0; i < bones.Length; i++)
{
Vector3 start = spheres[Array.IndexOf(sphereList, bonesList[2 * i])].transform.position;
Vector3 end = spheres[Array.IndexOf(sphereList, bonesList[2 * i + 1])].transform.position;
if (start == Vector3.zero || end == Vector3.zero)
{
bones[i].SetActive(false);
continue;
}
bones[i].SetActive(true);
Vector3 offset = end - start;
Vector3 scale = new Vector3(width, offset.magnitude / 2.0f, width);
Vector3 position = start + (offset / 2.0f);
bones[i].transform.position = position;
bones[i].transform.up = offset;
bones[i].transform.localScale = scale;
}
}
/// <summary>
/// For Debug only. Set the joint position as sphere.
/// </summary>
/// <param name="jt">Jt.</param>
public void setJointSpherePoint(Vector3[] jt)
{
if (sphere.Count != 18)
{
for (int i = 0; i < jointCount; i++)
{
sphere.Add(GameObject.CreatePrimitive(PrimitiveType.Sphere));
}
}
for (int i = 0; i < jointCount; i++)
{
if (ZEDSupportFunctions.IsVector3NaN(joint[i]))
{
continue;
}
joint[i] = new Vector3(jt[i].x, jt[i].y, jt[i].z);
sphere[i].transform.localScale = new Vector3(0.05f, 0.05f, 0.05f);
sphere[i].transform.position = joint[i];
}
}
/// <summary>
/// Gets the minimum distance to plane boundaries of a given 3D point (in world space)
/// </summary>
/// <returns>The minimum distance to boundaries.</returns>
/// <param name="worldPosition">World position.</param>
public float getMinimumDistanceToBoundaries(Camera cam, Vector3 worldPosition, out Vector3 minimumBoundsPosition)
{
Camera leftCamera = cam;
float minimal_distance = ZEDSupportFunctions.DistancePointLine(worldPosition, leftCamera.transform.TransformPoint(planeData.Bounds[0]), leftCamera.transform.TransformPoint(planeData.Bounds[1]));
Vector3 BestFoundPoint = new Vector3(0.0f, 0.0f, 0.0f);
if (planeData.BoundsSize > 2)
{
for (int i = 1; i < planeData.BoundsSize - 1; i++)
{
float currentDistance = ZEDSupportFunctions.DistancePointLine(worldPosition, leftCamera.transform.TransformPoint(planeData.Bounds[i]), leftCamera.transform.TransformPoint(planeData.Bounds[i + 1]));
if (currentDistance < minimal_distance)
{
minimal_distance = currentDistance;
BestFoundPoint = ZEDSupportFunctions.ProjectPointLine(worldPosition, leftCamera.transform.TransformPoint(planeData.Bounds[i]), leftCamera.transform.TransformPoint(planeData.Bounds[i + 1]));
}
}
}
minimumBoundsPosition = BestFoundPoint;
return(minimal_distance);
}
/// <summary>
/// Handles movements and collisions on a constant basis.
/// </summary>
void FixedUpdate()
{
//Calculate where the object should move this frame
Vector3 newpos = transform.position + transform.rotation * Vector3.forward * (Speed * Time.deltaTime);
//Collisions with the real World. As the object moves, collisions checks are made each frame at the next position.
Vector3 collisionpoint;
if (ZEDSupportFunctions.HitTestOnRay(_leftCamera, newpos, transform.rotation, Vector3.Distance(transform.position, newpos), DistanceBetweenRayChecks, out collisionpoint, false, RealWorldThickness))
{
//Call the function to resolve the impact. This allows us to easily override what happens on collisions with classes that inherit this one.
Vector3 collisionnormal;
ZEDSupportFunctions.GetNormalAtWorldLocation(collisionpoint, sl.REFERENCE_FRAME.WORLD, _leftCamera, out collisionnormal);
OnHitRealWorld(collisionpoint, collisionnormal);
}
//Collisions with virtual objects
//Cast a ray to check collisions between here and the intended move point for virtual objects.
RaycastHit hitinfo;
if (Physics.Raycast(transform.position, newpos - transform.position, out hitinfo, Vector3.Distance(transform.position, newpos)))
{
//Call the function to resolve the impact. This allows us to easily override what happens on collisions with classes that inherit this one.
OnHitVirtualWorld(hitinfo);
}
//Move it to this new place
transform.position = newpos;
//Tick down its lifespan and check if we should destroy it.
Lifespan -= Time.deltaTime;
if (Lifespan <= 0f)
{
Destroy(gameObject);
}
}
/// <summary>
/// Looks for a random point in a radius around itself.
/// Upon collision, the point is moved slightly towards the camera and if its too far it's set to "maxSpawnDistance".
/// A more thorough search is then done for any other obstacles around it, also, in a radius.
/// If the number of collision doesn't exeeds the set threshold, then return true and output the new position.
/// </summary>
/// <returns><c>true</c>, if random spawn location was checked, <c>false</c> otherwise.</returns>
/// <param name="newRandomPos">Random position.</param>
private bool CheckRandomSpawnLocation(out Vector3 newRandomPos)
{
//We can't do anything if the ZED isn't initialized.
if (!zedManager.IsZEDReady)
{
newRandomPos = Vector3.zero;
return(false);
}
//Pick a screen position at random between 0.25 and 0.75.
Vector2 randomScreenPoint = new Vector2(Random.Range(0.25f, 0.75f) * Screen.width, Random.Range(0.25f, 0.75f) * Screen.height);
//Get the world position of that position in the real world
Vector3 randomWorldPoint;
bool foundWorldPoint = ZEDSupportFunctions.GetWorldPositionAtPixel(zedManager.zedCamera, randomScreenPoint, leftcamera, out randomWorldPoint);
if (!foundWorldPoint) //We can't read depth from that point.
{
newRandomPos = Vector3.zero;
return(false);
}
float firstDistance = Vector3.Distance(leftcamera.transform.position, randomWorldPoint);
float newClearRadius;
//Check that the distance isn't too far.
if (firstDistance > maxSpawnDistance)
{
newClearRadius = firstDistance - maxSpawnDistance;
}
else
{
newClearRadius = clearRadius;
}
//If we spawn the drone at that world point, it'll spawn inside a wall. Bring it between you and that wall.
Quaternion directionToCamera = Quaternion.LookRotation(leftcamera.transform.position - randomWorldPoint, Vector3.up);
Vector3 closerWorldPoint = randomWorldPoint + directionToCamera * Vector3.forward * newClearRadius;
//Check that distance isn't too close
float secondDistance = Vector3.Distance(leftcamera.transform.position, closerWorldPoint);
if (secondDistance < 1f)
{
newRandomPos = Vector3.zero;
return(false);
}
//Also check nearby points in a sphere of radius=ClearRadius to make sure the whole drone has a clear space.
if (ZEDSupportFunctions.HitTestOnSphere(zedManager.zedCamera, leftcamera, closerWorldPoint, 1f, radiusCheckRate, percentagethreshold))
{
//Not clear.
newRandomPos = Vector3.zero;
return(false);
}
else
{
//Clear.
newRandomPos = closerWorldPoint;
return(true);
}
}
/// <summary>
/// Function that handles the humanoid position, rotation and bones movement
/// </summary>
/// <param name="position_center">Position center.</param>
private void setHumanPoseControl(Vector3 position_center)
{
Vector3 waist;
Quaternion waistrot = oldwaistrot;
Quaternion inv_waistrot = Quaternion.Inverse(waistrot);
if (!ZEDSupportFunctions.IsVector3NaN(joint[JointType_HipRight]) && !ZEDSupportFunctions.IsVector3NaN(joint[JointType_HipLeft]))
{
waist = joint[JointType_HipRight] - joint[JointType_HipLeft];
waist = new Vector3(waist.x, 0, waist.z);
waistrot = Quaternion.FromToRotation(Vector3.right, waist);
inv_waistrot = Quaternion.Inverse(waistrot);
}
Vector3 shoulder;
Quaternion shoulderrot = oldshoulderrot;
Quaternion inv_shoulderrot = Quaternion.Inverse(waistrot);
if (!ZEDSupportFunctions.IsVector3NaN(joint[JointType_ShoulderRight]) && !ZEDSupportFunctions.IsVector3NaN(joint[JointType_ShoulderLeft]))
{
shoulder = joint[JointType_ShoulderRight] - joint[JointType_ShoulderLeft];
shoulder = new Vector3(shoulder.x, 0, shoulder.z);
shoulderrot = Quaternion.FromToRotation(Vector3.right, shoulder);
inv_shoulderrot = Quaternion.Inverse(shoulderrot);
}
if (Quaternion.Angle(waistrot, shoulderrot) > 45 || Quaternion.Angle(waistrot, shoulderrot) < -45)
{
shoulderrot = oldshoulderrot;
}
for (int i = 0; i < targetBone.Length; i++)
{
int s = jointSegment[2 * i], e = jointSegment[2 * i + 1];
if (!ZEDSupportFunctions.IsVector3NaN(joint[e]) && !ZEDSupportFunctions.IsVector3NaN(joint[s]))
{
trackingSegment[targetBone[i]] = (joint[e] - joint[s]).normalized;
}
}
foreach (HumanBodyBones bone in targetBone)
{
rigBoneTarget[bone] = waistrot * Quaternion.identity;
}
Vector3 eyesVector = (joint[JointType_EyesLeft] + joint[JointType_HearLeft]) / 2 - (joint[JointType_EyesRight] + joint[JointType_HearRight]) / 2;
Vector3 headVector = joint[JointType_Head] - joint[JointType_Neck];
Vector3 headOrientationVector = Vector3.Cross(headVector, eyesVector);
if (headOrientationVector != Vector3.zero && headVector != Vector3.zero && !ZEDSupportFunctions.IsVector3NaN(headOrientationVector) && !ZEDSupportFunctions.IsVector3NaN(headVector))
{
rigBoneTarget[HumanBodyBones.Neck] = Quaternion.LookRotation(headOrientationVector, headVector);
}
else
{
rigBoneTarget[HumanBodyBones.Neck] = Quaternion.FromToRotation(shoulderrot * Vector3.up, trackingSegment[HumanBodyBones.Spine]) * shoulderrot;
}
rigBoneTarget[HumanBodyBones.Spine] = Quaternion.FromToRotation(waistrot * Vector3.up, trackingSegment[HumanBodyBones.Spine]) * waistrot;
rigBoneTarget[HumanBodyBones.LeftUpperArm] = Quaternion.FromToRotation(shoulderrot * Vector3.left, trackingSegment[HumanBodyBones.LeftUpperArm]) * shoulderrot;
rigBoneTarget[HumanBodyBones.LeftLowerArm] = Quaternion.FromToRotation(shoulderrot * Vector3.left, trackingSegment[HumanBodyBones.LeftLowerArm]) * shoulderrot;
rigBoneTarget[HumanBodyBones.RightUpperArm] = Quaternion.FromToRotation(shoulderrot * Vector3.right, trackingSegment[HumanBodyBones.RightUpperArm]) * shoulderrot;
rigBoneTarget[HumanBodyBones.RightLowerArm] = Quaternion.FromToRotation(shoulderrot * Vector3.right, trackingSegment[HumanBodyBones.RightLowerArm]) * shoulderrot;
rigBoneTarget[HumanBodyBones.LeftUpperLeg] = Quaternion.FromToRotation(waistrot * Vector3.down, trackingSegment[HumanBodyBones.LeftUpperLeg]) * waistrot;
rigBoneTarget[HumanBodyBones.LeftLowerLeg] = Quaternion.FromToRotation(waistrot * Vector3.down, trackingSegment[HumanBodyBones.LeftLowerLeg]) * waistrot;
rigBoneTarget[HumanBodyBones.RightUpperLeg] = Quaternion.FromToRotation(waistrot * Vector3.down, trackingSegment[HumanBodyBones.RightUpperLeg]) * waistrot;
rigBoneTarget[HumanBodyBones.RightLowerLeg] = Quaternion.FromToRotation(waistrot * Vector3.down, trackingSegment[HumanBodyBones.RightLowerLeg]) * waistrot;
rigBone[HumanBodyBones.UpperChest].offset(inv_waistrot * shoulderrot);
targetBodyOrientation = waistrot;
targetBodyPosition = new Vector3(position_center.x, position_center.y, position_center.z);
oldshoulderrot = shoulderrot;
oldwaistrot = waistrot;
}
