private static bool PolygonSet(PolygonMsg polygon)
{
if (polygon.GetPoints().Length == 0)
{
return(false);
}
return(true);
}
private void InitPlacePose()
{
//placePose = programItem.GetPose()[0].GetPose();
//placePosition = new Vector3(placePose.GetPosition().GetX(), -placePose.GetPosition().GetY(), placePose.GetPosition().GetZ() - 0.023f);
//placePosition = new Vector3(placePose.GetPosition().GetX(), -placePose.GetPosition().GetY(), 0f);
//placeQuaternion = new Quaternion(-placePose.GetOrientation().GetX(), placePose.GetOrientation().GetY(), -placePose.GetOrientation().GetZ(), placePose.GetOrientation().GetW());
//upPosition = objectToPlace.transform.localPosition + new Vector3(0,0,0.1f);
PolygonMsg polygonMsg = programItem.GetPolygon()[0].GetPolygon();
//compute centroid of polygon from which to pick
PointMsg[] _points = polygonMsg.GetPoints();
List <Vector2> points = new List <Vector2>();
for (int i = 0; i < _points.Length; i++)
{
points.Add(new Vector2(_points[i].GetX(), _points[i].GetY()));
}
centroid = GetCentroid(points);
placePosition = new Vector3(centroid.x, -centroid.y, objectToPlace.transform.GetChild(0).transform.localScale.x / 2f);
//rotate object to lay down on the table and to be parallel with world_anchor
if (objectToPlace.name.Equals("Stretcher") || objectToPlace.name.Equals("LongLeg") || objectToPlace.name.Equals("ShortLeg") ||
objectToPlace.name.Equals("Spojka") || objectToPlace.name.Equals("Dlouha_noha") || objectToPlace.name.Equals("Kratka_noha"))
{
placeQuaternion = Quaternion.Euler(new Vector3(-90f, 0f, 0f));
}
else
{
placeQuaternion = Quaternion.Euler(new Vector3(0f, 0f, 0f));
}
//get list of objects in place polygon in order to turn collisions off with the virtual rigidbody object
objectsInPolygon = ObjectsManager.Instance.GetObjectsFromPolygon(points);
//place it directly to container
foreach (var obj in objectsInPolygon)
{
if (obj.name.Contains("kontejner") || obj.name.Contains("container"))
{
placePosition = obj.transform.localPosition;
break;
}
}
upPosition = placePosition + new Vector3(0, 0, 0.3f);
}
private void InitObjectToPick()
{
List <GameObject> objectsInPolygon = new List <GameObject>();
//TODO: overit, ze v polygonu neni virtualni objekt .. pokud je, tak beru ho
PolygonMsg polygonMsg = programItem.GetPolygon()[0].GetPolygon();
//compute centroid of polygon from which to pick
PointMsg[] _points = polygonMsg.GetPoints();
List <Vector2> points = new List <Vector2>();
for (int i = 0; i < _points.Length; i++)
{
points.Add(new Vector2(_points[i].GetX(), _points[i].GetY()));
}
centroid = GetCentroid(points);
//find all virtual objects that might be in given polygon
objectsInPolygon = ObjectsManager.Instance.GetVirtualObjectsFromPolygon(points, programItem.GetObject()[0]);
//HIGHEST PRIORITY - we have some virtual objects placed in polygon
if (objectsInPolygon.Count > 0)
{
foreach (GameObject obj in objectsInPolygon)
{
if (obj.name.Equals(programItem.GetObject()[0]))
{
objectToPick = obj;
break;
}
}
}
//If no object created yet, then create new one and set everything needed for the freshly created object
else if (objectToPick == null)
{
//create empty parent object that has scale 1,1,1 .. when arm attaches it, it won't deform (cube for picking has different scale and it caused mess when arm attached it)
objectToPick = new GameObject();
//objectToPick is created at [0,0,0] world coords
//making him child of world_anchor leaves him still at [0,0,0] world coords.. but shifts it's localPosition to be in respect to world_anchor
//setting localPosition to [0,0,0] would make it appear on the same position as world_anchor
objectToPick.transform.parent = world_anchor.transform;
//instantiate actual object to pick and attach it to empty parent
GameObject childObjectToPick = Instantiate(visualizeObjectPrefab, objectToPick.transform);
childObjectToPick.transform.localScale = ObjectsManager.Instance.getObjectDimensions(programItem.GetObject()[0]);
childObjectToPick.transform.parent = objectToPick.transform;
objectToPick.transform.localPosition = new Vector3(0, 0, childObjectToPick.transform.localScale.x / 2f);
objectToPick.name = programItem.GetObject()[0];
//add reference on created object into ObjectsManagers list
ObjectsManager.Instance.virtualObjectList.Add(objectToPick);
//apply this centroid to init of object to pick
objectToPick.transform.localPosition = new Vector3(centroid.x, -centroid.y, objectToPick.transform.GetChild(0).transform.localScale.x / 2f);
//rotate object to lay down on the table and to be parallel with world_anchor
if (objectToPick.name.Equals("Stretcher") || objectToPick.name.Equals("LongLeg") || objectToPick.name.Equals("ShortLeg") ||
objectToPick.name.Equals("Spojka") || objectToPick.name.Equals("Dlouha_noha") || objectToPick.name.Equals("Kratka_noha"))
{
objectToPick.transform.localEulerAngles = new Vector3(-90f, 0f, 0f);
}
else
{
objectToPick.transform.localEulerAngles = new Vector3(0f, 0f, 0f);
}
}
}
//if there is no detected or virtual object in polygon, then it creates completely new one
private void InitObjectsToDrill()
{
objectsToDrill = new List <GameObject>();
objectsToDrillReferenceHolder = new List <GameObject>();
//get polygon points from current message
PolygonMsg polygonMsg = programItem.GetPolygon()[0].GetPolygon();
PointMsg[] _points = polygonMsg.GetPoints();
List <Vector2> points = new List <Vector2>();
for (int i = 0; i < _points.Length; i++)
{
points.Add(new Vector2(_points[i].GetX(), _points[i].GetY()));
}
//find all virtual objects that might be in given polygon
objectsInPolygon = ObjectsManager.Instance.GetVirtualObjectsFromPolygon(points, programItem.GetObject()[0]);
//HIGHEST PRIORITY - we have some virtual objects placed in polygon
if (objectsInPolygon.Count > 0)
{
objectsToDrill = new List <GameObject>(objectsInPolygon);
objectsToDrillReferenceHolder = new List <GameObject>(objectsInPolygon);
}
//MID PRIORITY - we try to find real objects that are in polygon
else
{
//find all physical objects that are in given polygon
objectsInPolygon = ObjectsManager.Instance.GetObjectsFromPolygon(points, programItem.GetObject()[0]);
//if in polygon are some detected objects .. instatiate on their poses virtual ones and drill all of them
if (objectsInPolygon.Count > 0)
{
Debug.Log("POLYGON NOT EMPTY!");
foreach (GameObject obj in objectsInPolygon)
{
GameObject objectToDrill = new GameObject();
objectToDrill.transform.parent = world_anchor.transform;
objectToDrill.transform.localPosition = obj.transform.localPosition;
objectToDrill.transform.localRotation = obj.transform.localRotation;
objectToDrill.name = programItem.GetObject()[0];
//instantiate actual object to drill and attach it to empty parent
GameObject childObjectToDrill = Instantiate(visualizeObjectPrefab, objectToDrill.transform);
childObjectToDrill.transform.localScale = ObjectsManager.Instance.getObjectDimensions(programItem.GetObject()[0]);
childObjectToDrill.transform.parent = objectToDrill.transform;
objectsToDrill.Add(objectToDrill);
objectsToDrillReferenceHolder.Add(objectToDrill);
//add reference on created object into ObjectsManagers list
ObjectsManager.Instance.virtualObjectList.Add(objectToDrill);
}
}
//LOWEST PRIORITY - if there are no objects in polygon.. then create virtual one
else
{
Debug.Log("POLYGON IS EMPTY!");
//create empty parent object that has scale 1,1,1 .. when arm attaches it, it won't deform (cube for picking has different scale and it caused mess when arm attached it)
GameObject objectToDrill = new GameObject();
//objectToDrill is created at [0,0,0] world coords
//making him child of world_anchor leaves him still at [0,0,0] world coords.. but shifts it's localPosition to be in respect to world_anchor
//setting localPosition to [0,0,0] would make it appear on the same position as world_anchor
objectToDrill.transform.parent = world_anchor.transform;
objectToDrill.transform.localPosition = new Vector3(0, 0, 0);
objectToDrill.name = programItem.GetObject()[0];
//compute centroid of polygon in which to drill
centroid = GetCentroid(points);
//apply this centroid to init of object to pick
objectToDrill.transform.localPosition = new Vector3(centroid.x, -centroid.y, 0);
//rotate object to lay down on the table and to be parallel with world_anchor
objectToDrill.transform.localEulerAngles = new Vector3(-90f, 0f, 0f);
//instantiate actual object to drill and attach it to empty parent
GameObject childObjectToDrill = Instantiate(visualizeObjectPrefab, objectToDrill.transform);
childObjectToDrill.transform.localScale = ObjectsManager.Instance.getObjectDimensions(programItem.GetObject()[0]);
childObjectToDrill.transform.parent = objectToDrill.transform;
objectsToDrill.Add(objectToDrill);
objectsToDrillReferenceHolder.Add(objectToDrill);
//add reference on created object into ObjectsManagers list
ObjectsManager.Instance.virtualObjectList.Add(objectToDrill);
}
}
}