bool FindNearestObstade(RayAgentData data, Vector3 target, out RayAgentHitInfo hitInfo, float distance, bool onlyCenter) { hitInfo = null; bool hasHit = false; float minDistance = Vector3.Distance(data.Position, target); Vector3 direction = target - data.Position; data.SetDirection(direction); Vector3[] anchors = data.Anchors; RaycastHit hit; for (int i = 0; i < anchors.Length; i++) { if (onlyCenter) { if (i != (int)RayAgentPos.Center) { continue; } } if (Physics.Raycast(anchors[i], direction, out hit, distance, layerMask.value)) { if (hit.transform == _target) { continue; } ObstacleBox obstacle = hit.transform.GetComponent <ObstacleBox>(); if (obstacle != null) { if (hit.distance < minDistance) { minDistance = hit.distance; if (hitInfo == null) { hitInfo = new RayAgentHitInfo(); } hitInfo.hit = hit; hitInfo.obstacle = obstacle; hitInfo.pos = (RayAgentPos)i; hitInfo.hitpoint = hit.point; if (hitInfo.pos == RayAgentPos.Right) { hitInfo.hitpoint += (data.Position - data.Right).normalized * data.radius; } else if (hitInfo.pos == RayAgentPos.Right) { hitInfo.hitpoint += (data.Position - data.Left).normalized * data.radius; } hasHit = true; } } } } return(hasHit); }
bool AnchorHit(Vector3 from, Vector3 to) { bool hasHit = false; RayAgentData data = new RayAgentData(); data.Position = from; data.radius *= 0.9F; float distance = Vector3.Distance(from, to) + extended; RayAgentHitInfo hitInfo; if (FindNearestObstade(data, to, out hitInfo, distance, false)) { hasHit = true; } return(hasHit); }
private bool Find(Vector3 from, out List <Vector3> paths) { bool enable = true; paths = new List <Vector3>(); RayAgentData data = new RayAgentData(); data.Position = from; RayAgentHitInfo hitInfo; RayAgentHitInfo hitInfo2; RayAgentHitInfo hitInfo3; if (FindNearestObstade(data, _end, out hitInfo, 100F, false)) { Vector3 hitPoint = Extended(hitInfo.hitpoint, _begin); int index; Vector3 obstacleAnchor = hitInfo.obstacle.FindAnchor(hitPoint, out index); bool hasAnchor = false; Vector3 anchor = obstacleAnchor; int anchorIndex = index; int index2 = -1; if (AnchorHit(hitPoint, obstacleAnchor)) { Vector3 obstacleAnchor2 = hitInfo.obstacle.FindAnchor2(hitPoint, index, out index2); if (AnchorHit(hitPoint, obstacleAnchor2)) { hasAnchor = false; //A* enable = false; return(enable); } else { anchorIndex = index2; anchor = obstacleAnchor2; hasAnchor = true; } // Gizmos.color = Color.cyan * 0.5F; // Gizmos.DrawWireSphere(obstacleAnchor2, 0.2F); } else { anchor = obstacleAnchor; hasAnchor = true; } // Gizmos.color = Color.green * 0.5F; // Gizmos.DrawWireSphere(hitInfo.hitpoint, 0.1F); // // Gizmos.color = Color.green; // Gizmos.DrawWireSphere(hitPoint, 0.1F); // // // Gizmos.color = Color.cyan; // Gizmos.DrawWireSphere(obstacleAnchor, 0.2F); if (hasAnchor) { data.Position = anchor; if (FindNearestObstade(data, _end, out hitInfo2, 100F, false)) { if (hitInfo2.hit.transform == hitInfo.hit.transform) { Vector3 obstacleAnchor3 = hitInfo.obstacle.FindAnchor3(hitPoint, anchorIndex); data.Position = obstacleAnchor3; if (FindNearestObstade(data, _end, out hitInfo3, 100F, true)) { if (hitInfo3.hit.transform != hitInfo.hit.transform) { paths.Add(hitPoint); paths.Add(anchor); paths.Add(obstacleAnchor3); List <Vector3> points; if (Find(obstacleAnchor3, out points)) { for (int i = 0; i < points.Count; i++) { paths.Add(points[i]); } } else { enabled = false; return(enabled); } } else { enabled = false; return(enabled); // paths.Add(hitPoint); // paths.Add(anchor); // paths.Add(obstacleAnchor3); //A* } } else { paths.Add(hitPoint); paths.Add(anchor); paths.Add(obstacleAnchor3); paths.Add(_end); } } else { paths.Add(hitPoint); paths.Add(anchor); List <Vector3> points; if (Find(anchor, out points)) { for (int i = 0; i < points.Count; i++) { paths.Add(points[i]); } } else { enabled = false; return(enabled); } } } else { paths.Add(hitPoint); paths.Add(anchor); paths.Add(_end); } } } else { paths.Add(_end); } return(enabled); }