590 lines
16 KiB
Go
590 lines
16 KiB
Go
package raycast
|
|
|
|
import (
|
|
"fmt"
|
|
"math"
|
|
"sync"
|
|
)
|
|
|
|
// RaycastMesh provides high-speed raycasting against triangle meshes using AABB trees
|
|
// This is a Go implementation based on the C++ raycast mesh system
|
|
type RaycastMesh struct {
|
|
vertices []float32 // Vertex positions (x,y,z,x,y,z,...)
|
|
indices []uint32 // Triangle indices (i1,i2,i3,i4,i5,i6,...)
|
|
grids []uint32 // Grid IDs for each triangle
|
|
widgets []uint32 // Widget IDs for each triangle
|
|
triangles []*Triangle // Processed triangles
|
|
root *AABBNode // Root of the AABB tree
|
|
boundMin [3]float32 // Minimum bounding box
|
|
boundMax [3]float32 // Maximum bounding box
|
|
maxDepth uint32 // Maximum tree depth
|
|
minLeafSize uint32 // Minimum triangles per leaf
|
|
minAxisSize float32 // Minimum axis size for subdivision
|
|
mutex sync.RWMutex
|
|
}
|
|
|
|
// Triangle represents a single triangle in the mesh
|
|
type Triangle struct {
|
|
Vertices [9]float32 // 3 vertices * 3 components (x,y,z)
|
|
Normal [3]float32 // Face normal
|
|
GridID uint32 // Associated grid ID
|
|
WidgetID uint32 // Associated widget ID
|
|
BoundMin [3]float32 // Triangle bounding box minimum
|
|
BoundMax [3]float32 // Triangle bounding box maximum
|
|
}
|
|
|
|
// AABBNode represents a node in the Axis-Aligned Bounding Box tree
|
|
type AABBNode struct {
|
|
BoundMin [3]float32 // Node bounding box minimum
|
|
BoundMax [3]float32 // Node bounding box maximum
|
|
Left *AABBNode // Left child (nil for leaf nodes)
|
|
Right *AABBNode // Right child (nil for leaf nodes)
|
|
Triangles []*Triangle // Triangles (only for leaf nodes)
|
|
Depth uint32 // Tree depth
|
|
}
|
|
|
|
// RaycastResult contains the results of a raycast operation
|
|
type RaycastResult struct {
|
|
Hit bool // Whether the ray hit something
|
|
HitLocation [3]float32 // World coordinates of hit point
|
|
HitNormal [3]float32 // Surface normal at hit point
|
|
HitDistance float32 // Distance from ray origin to hit
|
|
GridID uint32 // Grid ID of hit triangle
|
|
WidgetID uint32 // Widget ID of hit triangle
|
|
}
|
|
|
|
// RaycastOptions configures raycast behavior
|
|
type RaycastOptions struct {
|
|
IgnoredWidgets map[uint32]bool // Widget IDs to ignore during raycast
|
|
MaxDistance float32 // Maximum ray distance (0 = unlimited)
|
|
BothSides bool // Check both sides of triangles
|
|
}
|
|
|
|
// NewRaycastMesh creates a new raycast mesh from triangle data
|
|
func NewRaycastMesh(vertices []float32, indices []uint32, grids []uint32, widgets []uint32,
|
|
maxDepth uint32, minLeafSize uint32, minAxisSize float32) (*RaycastMesh, error) {
|
|
|
|
if len(vertices)%3 != 0 {
|
|
return nil, fmt.Errorf("vertex count must be divisible by 3")
|
|
}
|
|
if len(indices)%3 != 0 {
|
|
return nil, fmt.Errorf("index count must be divisible by 3")
|
|
}
|
|
|
|
triangleCount := len(indices) / 3
|
|
if len(grids) != triangleCount {
|
|
return nil, fmt.Errorf("grid count must match triangle count")
|
|
}
|
|
if len(widgets) != triangleCount {
|
|
return nil, fmt.Errorf("widget count must match triangle count")
|
|
}
|
|
|
|
rm := &RaycastMesh{
|
|
vertices: make([]float32, len(vertices)),
|
|
indices: make([]uint32, len(indices)),
|
|
grids: make([]uint32, len(grids)),
|
|
widgets: make([]uint32, len(widgets)),
|
|
triangles: make([]*Triangle, triangleCount),
|
|
maxDepth: maxDepth,
|
|
minLeafSize: minLeafSize,
|
|
minAxisSize: minAxisSize,
|
|
}
|
|
|
|
// Copy input data
|
|
copy(rm.vertices, vertices)
|
|
copy(rm.indices, indices)
|
|
copy(rm.grids, grids)
|
|
copy(rm.widgets, widgets)
|
|
|
|
// Process triangles
|
|
if err := rm.processTriangles(); err != nil {
|
|
return nil, fmt.Errorf("failed to process triangles: %v", err)
|
|
}
|
|
|
|
// Build AABB tree
|
|
if err := rm.buildAABBTree(); err != nil {
|
|
return nil, fmt.Errorf("failed to build AABB tree: %v", err)
|
|
}
|
|
|
|
return rm, nil
|
|
}
|
|
|
|
// Raycast performs optimized raycasting using the AABB tree
|
|
func (rm *RaycastMesh) Raycast(from, to [3]float32, options *RaycastOptions) *RaycastResult {
|
|
rm.mutex.RLock()
|
|
defer rm.mutex.RUnlock()
|
|
|
|
if options == nil {
|
|
options = &RaycastOptions{}
|
|
}
|
|
|
|
result := &RaycastResult{
|
|
Hit: false,
|
|
HitDistance: math.MaxFloat32,
|
|
}
|
|
|
|
// Calculate ray direction and length
|
|
rayDir := [3]float32{
|
|
to[0] - from[0],
|
|
to[1] - from[1],
|
|
to[2] - from[2],
|
|
}
|
|
|
|
rayLength := vectorLength(rayDir)
|
|
if rayLength < 1e-6 {
|
|
return result // Zero-length ray
|
|
}
|
|
|
|
// Normalize ray direction
|
|
rayDir[0] /= rayLength
|
|
rayDir[1] /= rayLength
|
|
rayDir[2] /= rayLength
|
|
|
|
// Use max distance if specified
|
|
maxDist := rayLength
|
|
if options.MaxDistance > 0 && options.MaxDistance < rayLength {
|
|
maxDist = options.MaxDistance
|
|
}
|
|
|
|
// Traverse AABB tree
|
|
rm.raycastNode(rm.root, from, rayDir, maxDist, options, result)
|
|
|
|
return result
|
|
}
|
|
|
|
// BruteForceRaycast performs raycast without spatial optimization (for testing/comparison)
|
|
func (rm *RaycastMesh) BruteForceRaycast(from, to [3]float32, options *RaycastOptions) *RaycastResult {
|
|
rm.mutex.RLock()
|
|
defer rm.mutex.RUnlock()
|
|
|
|
if options == nil {
|
|
options = &RaycastOptions{}
|
|
}
|
|
|
|
result := &RaycastResult{
|
|
Hit: false,
|
|
HitDistance: math.MaxFloat32,
|
|
}
|
|
|
|
rayDir := [3]float32{
|
|
to[0] - from[0],
|
|
to[1] - from[1],
|
|
to[2] - from[2],
|
|
}
|
|
|
|
rayLength := vectorLength(rayDir)
|
|
if rayLength < 1e-6 {
|
|
return result
|
|
}
|
|
|
|
rayDir[0] /= rayLength
|
|
rayDir[1] /= rayLength
|
|
rayDir[2] /= rayLength
|
|
|
|
maxDist := rayLength
|
|
if options.MaxDistance > 0 && options.MaxDistance < rayLength {
|
|
maxDist = options.MaxDistance
|
|
}
|
|
|
|
// Test all triangles
|
|
for _, triangle := range rm.triangles {
|
|
if options.IgnoredWidgets != nil && options.IgnoredWidgets[triangle.WidgetID] {
|
|
continue
|
|
}
|
|
|
|
if hitDist, hitPoint, hitNormal := rm.rayTriangleIntersect(from, rayDir, triangle, options.BothSides); hitDist >= 0 && hitDist <= maxDist && hitDist < result.HitDistance {
|
|
result.Hit = true
|
|
result.HitDistance = hitDist
|
|
result.HitLocation = hitPoint
|
|
result.HitNormal = hitNormal
|
|
result.GridID = triangle.GridID
|
|
result.WidgetID = triangle.WidgetID
|
|
}
|
|
}
|
|
|
|
return result
|
|
}
|
|
|
|
// GetBoundMin returns the minimum bounding box coordinates
|
|
func (rm *RaycastMesh) GetBoundMin() [3]float32 {
|
|
rm.mutex.RLock()
|
|
defer rm.mutex.RUnlock()
|
|
return rm.boundMin
|
|
}
|
|
|
|
// GetBoundMax returns the maximum bounding box coordinates
|
|
func (rm *RaycastMesh) GetBoundMax() [3]float32 {
|
|
rm.mutex.RLock()
|
|
defer rm.mutex.RUnlock()
|
|
return rm.boundMax
|
|
}
|
|
|
|
// GetTriangleCount returns the number of triangles in the mesh
|
|
func (rm *RaycastMesh) GetTriangleCount() int {
|
|
rm.mutex.RLock()
|
|
defer rm.mutex.RUnlock()
|
|
return len(rm.triangles)
|
|
}
|
|
|
|
// GetTreeDepth returns the actual depth of the AABB tree
|
|
func (rm *RaycastMesh) GetTreeDepth() uint32 {
|
|
rm.mutex.RLock()
|
|
defer rm.mutex.RUnlock()
|
|
if rm.root == nil {
|
|
return 0
|
|
}
|
|
return rm.getNodeDepth(rm.root)
|
|
}
|
|
|
|
// Private methods
|
|
|
|
func (rm *RaycastMesh) processTriangles() error {
|
|
rm.boundMin = [3]float32{math.MaxFloat32, math.MaxFloat32, math.MaxFloat32}
|
|
rm.boundMax = [3]float32{-math.MaxFloat32, -math.MaxFloat32, -math.MaxFloat32}
|
|
|
|
for i := 0; i < len(rm.indices); i += 3 {
|
|
triangle := &Triangle{
|
|
GridID: rm.grids[i/3],
|
|
WidgetID: rm.widgets[i/3],
|
|
}
|
|
|
|
// Get vertex indices
|
|
i1, i2, i3 := rm.indices[i], rm.indices[i+1], rm.indices[i+2]
|
|
|
|
// Validate indices
|
|
if i1*3+2 >= uint32(len(rm.vertices)) || i2*3+2 >= uint32(len(rm.vertices)) || i3*3+2 >= uint32(len(rm.vertices)) {
|
|
return fmt.Errorf("invalid vertex index in triangle %d", i/3)
|
|
}
|
|
|
|
// Copy vertex positions
|
|
copy(triangle.Vertices[0:3], rm.vertices[i1*3:i1*3+3])
|
|
copy(triangle.Vertices[3:6], rm.vertices[i2*3:i2*3+3])
|
|
copy(triangle.Vertices[6:9], rm.vertices[i3*3:i3*3+3])
|
|
|
|
// Calculate triangle bounding box
|
|
triangle.BoundMin = [3]float32{math.MaxFloat32, math.MaxFloat32, math.MaxFloat32}
|
|
triangle.BoundMax = [3]float32{-math.MaxFloat32, -math.MaxFloat32, -math.MaxFloat32}
|
|
|
|
for j := 0; j < 9; j += 3 {
|
|
for k := 0; k < 3; k++ {
|
|
if triangle.Vertices[j+k] < triangle.BoundMin[k] {
|
|
triangle.BoundMin[k] = triangle.Vertices[j+k]
|
|
}
|
|
if triangle.Vertices[j+k] > triangle.BoundMax[k] {
|
|
triangle.BoundMax[k] = triangle.Vertices[j+k]
|
|
}
|
|
}
|
|
}
|
|
|
|
// Update global bounding box
|
|
for k := 0; k < 3; k++ {
|
|
if triangle.BoundMin[k] < rm.boundMin[k] {
|
|
rm.boundMin[k] = triangle.BoundMin[k]
|
|
}
|
|
if triangle.BoundMax[k] > rm.boundMax[k] {
|
|
rm.boundMax[k] = triangle.BoundMax[k]
|
|
}
|
|
}
|
|
|
|
// Calculate face normal
|
|
rm.calculateTriangleNormal(triangle)
|
|
|
|
rm.triangles[i/3] = triangle
|
|
}
|
|
|
|
return nil
|
|
}
|
|
|
|
func (rm *RaycastMesh) calculateTriangleNormal(triangle *Triangle) {
|
|
// Calculate two edge vectors
|
|
edge1 := [3]float32{
|
|
triangle.Vertices[3] - triangle.Vertices[0],
|
|
triangle.Vertices[4] - triangle.Vertices[1],
|
|
triangle.Vertices[5] - triangle.Vertices[2],
|
|
}
|
|
|
|
edge2 := [3]float32{
|
|
triangle.Vertices[6] - triangle.Vertices[0],
|
|
triangle.Vertices[7] - triangle.Vertices[1],
|
|
triangle.Vertices[8] - triangle.Vertices[2],
|
|
}
|
|
|
|
// Cross product
|
|
triangle.Normal[0] = edge1[1]*edge2[2] - edge1[2]*edge2[1]
|
|
triangle.Normal[1] = edge1[2]*edge2[0] - edge1[0]*edge2[2]
|
|
triangle.Normal[2] = edge1[0]*edge2[1] - edge1[1]*edge2[0]
|
|
|
|
// Normalize
|
|
length := vectorLength(triangle.Normal)
|
|
if length > 1e-6 {
|
|
triangle.Normal[0] /= length
|
|
triangle.Normal[1] /= length
|
|
triangle.Normal[2] /= length
|
|
}
|
|
}
|
|
|
|
func (rm *RaycastMesh) buildAABBTree() error {
|
|
if len(rm.triangles) == 0 {
|
|
return fmt.Errorf("no triangles to build tree from")
|
|
}
|
|
|
|
// Create root node with all triangles
|
|
rm.root = &AABBNode{
|
|
BoundMin: rm.boundMin,
|
|
BoundMax: rm.boundMax,
|
|
Triangles: rm.triangles,
|
|
Depth: 0,
|
|
}
|
|
|
|
// Recursively subdivide
|
|
rm.subdivideNode(rm.root)
|
|
|
|
return nil
|
|
}
|
|
|
|
func (rm *RaycastMesh) subdivideNode(node *AABBNode) {
|
|
// Stop subdivision if we've reached limits
|
|
if node.Depth >= rm.maxDepth || uint32(len(node.Triangles)) <= rm.minLeafSize {
|
|
return
|
|
}
|
|
|
|
// Find longest axis
|
|
size := [3]float32{
|
|
node.BoundMax[0] - node.BoundMin[0],
|
|
node.BoundMax[1] - node.BoundMin[1],
|
|
node.BoundMax[2] - node.BoundMin[2],
|
|
}
|
|
|
|
axis := 0
|
|
if size[1] > size[axis] {
|
|
axis = 1
|
|
}
|
|
if size[2] > size[axis] {
|
|
axis = 2
|
|
}
|
|
|
|
// Stop if axis is too small
|
|
if size[axis] < rm.minAxisSize {
|
|
return
|
|
}
|
|
|
|
// Split at midpoint
|
|
split := node.BoundMin[axis] + size[axis]*0.5
|
|
|
|
// Partition triangles
|
|
var leftTriangles, rightTriangles []*Triangle
|
|
for _, triangle := range node.Triangles {
|
|
center := (triangle.BoundMin[axis] + triangle.BoundMax[axis]) * 0.5
|
|
if center < split {
|
|
leftTriangles = append(leftTriangles, triangle)
|
|
} else {
|
|
rightTriangles = append(rightTriangles, triangle)
|
|
}
|
|
}
|
|
|
|
// Make sure both sides have triangles
|
|
if len(leftTriangles) == 0 || len(rightTriangles) == 0 {
|
|
return
|
|
}
|
|
|
|
// Create child nodes
|
|
node.Left = &AABBNode{
|
|
Triangles: leftTriangles,
|
|
Depth: node.Depth + 1,
|
|
}
|
|
node.Right = &AABBNode{
|
|
Triangles: rightTriangles,
|
|
Depth: node.Depth + 1,
|
|
}
|
|
|
|
// Calculate child bounding boxes
|
|
rm.calculateNodeBounds(node.Left)
|
|
rm.calculateNodeBounds(node.Right)
|
|
|
|
// Clear triangles from internal node
|
|
node.Triangles = nil
|
|
|
|
// Recursively subdivide children
|
|
rm.subdivideNode(node.Left)
|
|
rm.subdivideNode(node.Right)
|
|
}
|
|
|
|
func (rm *RaycastMesh) calculateNodeBounds(node *AABBNode) {
|
|
if len(node.Triangles) == 0 {
|
|
return
|
|
}
|
|
|
|
node.BoundMin = [3]float32{math.MaxFloat32, math.MaxFloat32, math.MaxFloat32}
|
|
node.BoundMax = [3]float32{-math.MaxFloat32, -math.MaxFloat32, -math.MaxFloat32}
|
|
|
|
for _, triangle := range node.Triangles {
|
|
for k := 0; k < 3; k++ {
|
|
if triangle.BoundMin[k] < node.BoundMin[k] {
|
|
node.BoundMin[k] = triangle.BoundMin[k]
|
|
}
|
|
if triangle.BoundMax[k] > node.BoundMax[k] {
|
|
node.BoundMax[k] = triangle.BoundMax[k]
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
func (rm *RaycastMesh) raycastNode(node *AABBNode, rayOrigin, rayDir [3]float32, maxDist float32,
|
|
options *RaycastOptions, result *RaycastResult) {
|
|
|
|
if node == nil {
|
|
return
|
|
}
|
|
|
|
// Test ray against node bounding box
|
|
if !rm.rayAABBIntersect(rayOrigin, rayDir, node.BoundMin, node.BoundMax, maxDist) {
|
|
return
|
|
}
|
|
|
|
// Leaf node - test triangles
|
|
if node.Left == nil && node.Right == nil {
|
|
for _, triangle := range node.Triangles {
|
|
if options.IgnoredWidgets != nil && options.IgnoredWidgets[triangle.WidgetID] {
|
|
continue
|
|
}
|
|
|
|
if hitDist, hitPoint, hitNormal := rm.rayTriangleIntersect(rayOrigin, rayDir, triangle, options.BothSides); hitDist >= 0 && hitDist <= maxDist && hitDist < result.HitDistance {
|
|
result.Hit = true
|
|
result.HitDistance = hitDist
|
|
result.HitLocation = hitPoint
|
|
result.HitNormal = hitNormal
|
|
result.GridID = triangle.GridID
|
|
result.WidgetID = triangle.WidgetID
|
|
}
|
|
}
|
|
} else {
|
|
// Internal node - recurse to children
|
|
rm.raycastNode(node.Left, rayOrigin, rayDir, maxDist, options, result)
|
|
rm.raycastNode(node.Right, rayOrigin, rayDir, maxDist, options, result)
|
|
}
|
|
}
|
|
|
|
func (rm *RaycastMesh) rayAABBIntersect(rayOrigin, rayDir [3]float32, boundMin, boundMax [3]float32, maxDist float32) bool {
|
|
var tMin, tMax float32 = 0, maxDist
|
|
|
|
for i := 0; i < 3; i++ {
|
|
if math.Abs(float64(rayDir[i])) < 1e-6 {
|
|
// Ray is parallel to axis
|
|
if rayOrigin[i] < boundMin[i] || rayOrigin[i] > boundMax[i] {
|
|
return false
|
|
}
|
|
} else {
|
|
// Calculate intersection distances
|
|
invDir := 1.0 / rayDir[i]
|
|
t1 := (boundMin[i] - rayOrigin[i]) * invDir
|
|
t2 := (boundMax[i] - rayOrigin[i]) * invDir
|
|
|
|
if t1 > t2 {
|
|
t1, t2 = t2, t1
|
|
}
|
|
|
|
tMin = float32(math.Max(float64(tMin), float64(t1)))
|
|
tMax = float32(math.Min(float64(tMax), float64(t2)))
|
|
|
|
if tMin > tMax {
|
|
return false
|
|
}
|
|
}
|
|
}
|
|
|
|
return tMin <= maxDist
|
|
}
|
|
|
|
func (rm *RaycastMesh) rayTriangleIntersect(rayOrigin, rayDir [3]float32, triangle *Triangle, bothSides bool) (float32, [3]float32, [3]float32) {
|
|
// Möller-Trumbore ray-triangle intersection algorithm
|
|
|
|
// Get triangle vertices
|
|
v0 := [3]float32{triangle.Vertices[0], triangle.Vertices[1], triangle.Vertices[2]}
|
|
v1 := [3]float32{triangle.Vertices[3], triangle.Vertices[4], triangle.Vertices[5]}
|
|
v2 := [3]float32{triangle.Vertices[6], triangle.Vertices[7], triangle.Vertices[8]}
|
|
|
|
// Edge vectors
|
|
edge1 := [3]float32{v1[0] - v0[0], v1[1] - v0[1], v1[2] - v0[2]}
|
|
edge2 := [3]float32{v2[0] - v0[0], v2[1] - v0[1], v2[2] - v0[2]}
|
|
|
|
// Cross product of ray direction and edge2
|
|
h := [3]float32{
|
|
rayDir[1]*edge2[2] - rayDir[2]*edge2[1],
|
|
rayDir[2]*edge2[0] - rayDir[0]*edge2[2],
|
|
rayDir[0]*edge2[1] - rayDir[1]*edge2[0],
|
|
}
|
|
|
|
// Dot product of edge1 and h
|
|
a := edge1[0]*h[0] + edge1[1]*h[1] + edge1[2]*h[2]
|
|
|
|
if math.Abs(float64(a)) < 1e-6 {
|
|
return -1, [3]float32{}, [3]float32{} // Ray is parallel to triangle
|
|
}
|
|
|
|
f := 1.0 / a
|
|
s := [3]float32{rayOrigin[0] - v0[0], rayOrigin[1] - v0[1], rayOrigin[2] - v0[2]}
|
|
u := f * (s[0]*h[0] + s[1]*h[1] + s[2]*h[2])
|
|
|
|
if u < 0.0 || u > 1.0 {
|
|
return -1, [3]float32{}, [3]float32{}
|
|
}
|
|
|
|
q := [3]float32{
|
|
s[1]*edge1[2] - s[2]*edge1[1],
|
|
s[2]*edge1[0] - s[0]*edge1[2],
|
|
s[0]*edge1[1] - s[1]*edge1[0],
|
|
}
|
|
|
|
v := f * (rayDir[0]*q[0] + rayDir[1]*q[1] + rayDir[2]*q[2])
|
|
|
|
if v < 0.0 || u+v > 1.0 {
|
|
return -1, [3]float32{}, [3]float32{}
|
|
}
|
|
|
|
t := f * (edge2[0]*q[0] + edge2[1]*q[1] + edge2[2]*q[2])
|
|
|
|
if t < 1e-6 { // Ray intersection behind origin
|
|
return -1, [3]float32{}, [3]float32{}
|
|
}
|
|
|
|
// Check backface culling
|
|
if !bothSides && a > 0 {
|
|
return -1, [3]float32{}, [3]float32{}
|
|
}
|
|
|
|
// Calculate hit point
|
|
hitPoint := [3]float32{
|
|
rayOrigin[0] + rayDir[0]*t,
|
|
rayOrigin[1] + rayDir[1]*t,
|
|
rayOrigin[2] + rayDir[2]*t,
|
|
}
|
|
|
|
// Use precomputed normal
|
|
hitNormal := triangle.Normal
|
|
|
|
return t, hitPoint, hitNormal
|
|
}
|
|
|
|
func (rm *RaycastMesh) getNodeDepth(node *AABBNode) uint32 {
|
|
if node == nil {
|
|
return 0
|
|
}
|
|
|
|
if node.Left == nil && node.Right == nil {
|
|
return node.Depth
|
|
}
|
|
|
|
leftDepth := rm.getNodeDepth(node.Left)
|
|
rightDepth := rm.getNodeDepth(node.Right)
|
|
|
|
if leftDepth > rightDepth {
|
|
return leftDepth
|
|
}
|
|
return rightDepth
|
|
}
|
|
|
|
// Utility functions
|
|
|
|
func vectorLength(v [3]float32) float32 {
|
|
return float32(math.Sqrt(float64(v[0]*v[0] + v[1]*v[1] + v[2]*v[2])))
|
|
} |