新增SensorToolkit
This commit is contained in:
164
Assets/SensorToolkit/Sensors/FOVCollider.cs
Normal file
164
Assets/SensorToolkit/Sensors/FOVCollider.cs
Normal file
@@ -0,0 +1,164 @@
|
||||
using UnityEngine;
|
||||
using System.Collections;
|
||||
|
||||
namespace Micosmo.SensorToolkit
|
||||
{
|
||||
/*
|
||||
* A paramemtric shape for creating field of view cones that work with the trigger sensor. Requires a MeshCollider
|
||||
* component on the same gameobject. When the script starts it will dynamically create a mesh for the fov cone and
|
||||
* assign it to this MeshCollider component.
|
||||
*/
|
||||
[RequireComponent(typeof(MeshCollider))]
|
||||
[ExecuteInEditMode]
|
||||
[HelpURL("https://micosmo.com/sensortoolkit2/docs/manual/fov")]
|
||||
public class FOVCollider : MonoBehaviour {
|
||||
[Tooltip("The length of the field of view cone in world units.")]
|
||||
public float Length = 5f;
|
||||
|
||||
[Tooltip("The distance to the near plane of the frustum.")]
|
||||
public float NearDistance = 0.1f;
|
||||
|
||||
[Range(1f, 180f), Tooltip("The arc angle of the fov cone.")]
|
||||
public float FOVAngle = 90f;
|
||||
|
||||
[Range(1f, 180f), Tooltip("The elevation angle of the cone.")]
|
||||
public float ElevationAngle = 90f;
|
||||
|
||||
[Range(0, 8), Tooltip("The number of vertices used to approximate the arc of the fov cone. Ideally this should be as low as possible.")]
|
||||
public int Resolution = 0;
|
||||
|
||||
// Returns the generated collider mesh so that it can be rendered.
|
||||
public Mesh FOVMesh => mesh;
|
||||
|
||||
Mesh mesh;
|
||||
MeshCollider mc;
|
||||
Vector3[] pts;
|
||||
int[] triangles;
|
||||
|
||||
void Awake() {
|
||||
mc = GetComponent<MeshCollider>();
|
||||
CreateCollider();
|
||||
}
|
||||
|
||||
void OnValidate() {
|
||||
Length = Mathf.Max(0f, Length);
|
||||
NearDistance = Mathf.Clamp(NearDistance, 0f, Length);
|
||||
if (mc != null) {
|
||||
CreateCollider();
|
||||
}
|
||||
}
|
||||
|
||||
public void CreateCollider() {
|
||||
pts = new Vector3[4 + (2+Resolution)*(2+Resolution)];
|
||||
// There are 2 triangles on the base
|
||||
var baseTriangleIndices = 2 * 3;
|
||||
// The arc is (Resolution+2) vertices to each side, making (Resolution+1)*(Resolution+1) boxes of 2 tris each
|
||||
var arcTriangleIndices = (Resolution + 1) * (Resolution + 1) * 2 * 3;
|
||||
// There are 4 sides to the cone, and each side has Resolution+2 triangles
|
||||
var sideTriangleIndices = (Resolution + 2) * 3;
|
||||
triangles = new int[baseTriangleIndices + arcTriangleIndices + sideTriangleIndices*4];
|
||||
|
||||
// Base points
|
||||
pts[0] = Quaternion.Euler(-ElevationAngle / 2f, -FOVAngle / 2f, 0f) * Vector3.forward * NearDistance; // Top Left
|
||||
pts[1] = Quaternion.Euler(ElevationAngle / 2f, -FOVAngle / 2f, 0f) * Vector3.forward * NearDistance; // Bottom Left
|
||||
pts[2] = Quaternion.Euler(ElevationAngle / 2f, FOVAngle / 2f, 0f) * Vector3.forward * NearDistance; // Bottom Right
|
||||
pts[3] = Quaternion.Euler(-ElevationAngle / 2f, FOVAngle / 2f, 0f) * Vector3.forward * NearDistance; // Top Right
|
||||
triangles[0] = 2; triangles[1] = 1; triangles[2] = 0; triangles[3] = 3; triangles[4] = 2; triangles[5] = 0;
|
||||
|
||||
for (int y = 0; y < 2+Resolution; y++) {
|
||||
for (int x = 0; x < 2+Resolution; x++) {
|
||||
int i = 4 + y * (2 + Resolution) + x;
|
||||
float ay = Mathf.Lerp(-FOVAngle / 2f, FOVAngle / 2f, (float)x / (float)(Resolution + 1));
|
||||
float ax = Mathf.Lerp(-ElevationAngle / 2f, ElevationAngle / 2f, (float)y / (float)(Resolution + 1));
|
||||
Vector3 p = Quaternion.Euler(ax, ay, 0f) * Vector3.forward * Length;
|
||||
pts[i] = p;
|
||||
|
||||
if (x < (1+Resolution) && y < (1+Resolution)) {
|
||||
var ti = baseTriangleIndices + (y * (Resolution + 1) + x) * 3 * 2;
|
||||
triangles[ti] = i + 1 + (2 + Resolution); // top right
|
||||
triangles[ti + 1] = i + 1; // bottom right
|
||||
triangles[ti + 2] = i; // bottom left
|
||||
triangles[ti + 3] = i + (2 + Resolution); // top left
|
||||
triangles[ti + 4] = i + (2 + Resolution) + 1; // top right
|
||||
triangles[ti + 5] = i; // bottom left
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Top and bottom side triangles
|
||||
for (int x = 0; x < 2+Resolution; x++) {
|
||||
var iTop = 4 + x;
|
||||
var iBottom = 4 + (1 + Resolution) * (2 + Resolution) + x;
|
||||
|
||||
var tiTop = baseTriangleIndices + arcTriangleIndices + x*3;
|
||||
var tiBottom = tiTop + sideTriangleIndices;
|
||||
if (x == 0) {
|
||||
triangles[tiTop] = 3;
|
||||
triangles[tiTop+1] = 0;
|
||||
triangles[tiTop + 2] = iTop;
|
||||
|
||||
triangles[tiBottom] = 1;
|
||||
triangles[tiBottom + 1] = 2;
|
||||
triangles[tiBottom + 2] = iBottom;
|
||||
} else {
|
||||
triangles[tiTop] = iTop;
|
||||
triangles[tiTop + 1] = 3;
|
||||
triangles[tiTop + 2] = iTop-1;
|
||||
|
||||
triangles[tiBottom] = 2;
|
||||
triangles[tiBottom + 1] = iBottom;
|
||||
triangles[tiBottom + 2] = iBottom-1;
|
||||
}
|
||||
}
|
||||
|
||||
// Left and right side triangles
|
||||
var yIncr = 2 + Resolution;
|
||||
for (int y = 0; y < 2 + Resolution; y++) {
|
||||
var iLeft = 4 + y*(2+Resolution);
|
||||
var iRight = iLeft + (1+Resolution);
|
||||
|
||||
var tiLeft = baseTriangleIndices + arcTriangleIndices + sideTriangleIndices*2 + y*3;
|
||||
var tiRight = tiLeft + sideTriangleIndices;
|
||||
if (y == 0) {
|
||||
triangles[tiLeft] = 0;
|
||||
triangles[tiLeft + 1] = 1;
|
||||
triangles[tiLeft + 2] = iLeft;
|
||||
|
||||
triangles[tiRight] = 2;
|
||||
triangles[tiRight + 1] = 3;
|
||||
triangles[tiRight + 2] = iRight;
|
||||
} else {
|
||||
triangles[tiLeft] = 1;
|
||||
triangles[tiLeft + 1] = iLeft;
|
||||
triangles[tiLeft + 2] = iLeft - yIncr;
|
||||
|
||||
triangles[tiRight] = iRight;
|
||||
triangles[tiRight + 1] = 2;
|
||||
triangles[tiRight + 2] = iRight - yIncr;
|
||||
}
|
||||
}
|
||||
|
||||
releaseMesh();
|
||||
mesh = new Mesh();
|
||||
mesh.vertices = pts;
|
||||
mesh.triangles = triangles;
|
||||
mesh.name = "FOVColliderPoints";
|
||||
mc.sharedMesh = mesh;
|
||||
mc.convex = true;
|
||||
mc.isTrigger = true;
|
||||
}
|
||||
|
||||
void releaseMesh() {
|
||||
if (mc.sharedMesh != null && mc.sharedMesh == mesh) {
|
||||
DestroyImmediate(mc.sharedMesh, true);
|
||||
}
|
||||
}
|
||||
|
||||
void OnDrawGizmosSelected() {
|
||||
Gizmos.color = Color.green;
|
||||
foreach(Vector3 p in pts) {
|
||||
Gizmos.DrawSphere(transform.TransformPoint(p), 0.02f);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user