Add CCLite 0.2.3 geometry primitives

This commit is contained in:
Codex
2026-07-05 23:55:23 +08:00
parent aceacc0acd
commit 5a4af31894
4 changed files with 545 additions and 12 deletions
+372
View File
@@ -0,0 +1,372 @@
module ccl_geometry
use, intrinsic :: ieee_arithmetic, only: ieee_is_finite
use iso_c_binding, only: c_double
use ccl_errors, only: CCL_ERROR_INPUT_INVALID_VALUE, CCL_SUCCESS
implicit none
real(c_double), parameter :: CCL_GEOM_EPS = 1.0e-12_c_double
type :: CCL_AABB_Type
logical :: is_valid = .false.
real(c_double) :: min_corner(3) = 0.0_c_double
real(c_double) :: max_corner(3) = 0.0_c_double
end type CCL_AABB_Type
type :: CCL_Geometry_Diagnostics_Type
integer :: degenerate_edge_count = 0
integer :: degenerate_triangle_count = 0
integer :: invalid_normal_count = 0
end type CCL_Geometry_Diagnostics_Type
contains
real(c_double) function ccl_vec3_dot(a, b) result(value)
real(c_double), intent(in) :: a(3)
real(c_double), intent(in) :: b(3)
value = sum(a * b)
end function ccl_vec3_dot
subroutine ccl_vec3_cross(a, b, value)
real(c_double), intent(in) :: a(3)
real(c_double), intent(in) :: b(3)
real(c_double), intent(out) :: value(3)
value(1) = a(2) * b(3) - a(3) * b(2)
value(2) = a(3) * b(1) - a(1) * b(3)
value(3) = a(1) * b(2) - a(2) * b(1)
end subroutine ccl_vec3_cross
real(c_double) function ccl_vec3_norm(a) result(value)
real(c_double), intent(in) :: a(3)
value = sqrt(ccl_vec3_dot(a, a))
end function ccl_vec3_norm
subroutine ccl_vec3_normalize(a, value, ierr, diagnostics)
real(c_double), intent(in) :: a(3)
real(c_double), intent(out) :: value(3)
integer, intent(out) :: ierr
type(CCL_Geometry_Diagnostics_Type), intent(inout), optional :: diagnostics
real(c_double) :: norm_value
ierr = CCL_SUCCESS
value = 0.0_c_double
if (.not. finite_vector3(a)) then
ierr = CCL_ERROR_INPUT_INVALID_VALUE
if (present(diagnostics)) diagnostics%invalid_normal_count = diagnostics%invalid_normal_count + 1
return
end if
norm_value = ccl_vec3_norm(a)
if (norm_value <= CCL_GEOM_EPS) then
ierr = CCL_ERROR_INPUT_INVALID_VALUE
if (present(diagnostics)) diagnostics%invalid_normal_count = diagnostics%invalid_normal_count + 1
return
end if
value = a / norm_value
end subroutine ccl_vec3_normalize
subroutine ccl_aabb_init(aabb)
type(CCL_AABB_Type), intent(out) :: aabb
aabb%is_valid = .false.
aabb%min_corner = huge(0.0_c_double)
aabb%max_corner = -huge(0.0_c_double)
end subroutine ccl_aabb_init
subroutine ccl_aabb_expand_point(aabb, point, ierr)
type(CCL_AABB_Type), intent(inout) :: aabb
real(c_double), intent(in) :: point(3)
integer, intent(out) :: ierr
ierr = CCL_SUCCESS
if (.not. finite_vector3(point)) then
ierr = CCL_ERROR_INPUT_INVALID_VALUE
return
end if
if (.not. aabb%is_valid) then
aabb%min_corner = point
aabb%max_corner = point
aabb%is_valid = .true.
else
aabb%min_corner = min(aabb%min_corner, point)
aabb%max_corner = max(aabb%max_corner, point)
end if
end subroutine ccl_aabb_expand_point
subroutine ccl_aabb_expand_radius(aabb, point, radius, ierr)
type(CCL_AABB_Type), intent(inout) :: aabb
real(c_double), intent(in) :: point(3)
real(c_double), intent(in) :: radius
integer, intent(out) :: ierr
if (.not. ieee_is_finite(radius) .or. radius < 0.0_c_double) then
ierr = CCL_ERROR_INPUT_INVALID_VALUE
return
end if
call ccl_aabb_expand_point(aabb, point - radius, ierr)
if (ierr /= CCL_SUCCESS) return
call ccl_aabb_expand_point(aabb, point + radius, ierr)
end subroutine ccl_aabb_expand_radius
logical function ccl_aabb_overlap(left, right) result(overlap)
type(CCL_AABB_Type), intent(in) :: left
type(CCL_AABB_Type), intent(in) :: right
overlap = .false.
if (.not. left%is_valid .or. .not. right%is_valid) return
overlap = all(left%max_corner >= right%min_corner) .and. all(right%max_corner >= left%min_corner)
end function ccl_aabb_overlap
subroutine ccl_particle_aabb(x, contact_radius, aabb, ierr)
real(c_double), intent(in) :: x(3)
real(c_double), intent(in) :: contact_radius
type(CCL_AABB_Type), intent(out) :: aabb
integer, intent(out) :: ierr
call ccl_aabb_init(aabb)
call ccl_aabb_expand_radius(aabb, x, contact_radius, ierr)
end subroutine ccl_particle_aabb
subroutine ccl_facet_aabb(x, node_count, padding, aabb, ierr)
real(c_double), intent(in) :: x(3, 8)
integer, intent(in) :: node_count
real(c_double), intent(in) :: padding
type(CCL_AABB_Type), intent(out) :: aabb
integer, intent(out) :: ierr
integer :: i
call ccl_aabb_init(aabb)
if (node_count < 3 .or. node_count > 8) then
ierr = CCL_ERROR_INPUT_INVALID_VALUE
return
end if
if (.not. ieee_is_finite(padding) .or. padding < 0.0_c_double) then
ierr = CCL_ERROR_INPUT_INVALID_VALUE
return
end if
do i = 1, node_count
call ccl_aabb_expand_point(aabb, x(:, i), ierr)
if (ierr /= CCL_SUCCESS) return
end do
if (padding > 0.0_c_double) then
aabb%min_corner = aabb%min_corner - padding
aabb%max_corner = aabb%max_corner + padding
end if
end subroutine ccl_facet_aabb
subroutine ccl_point_segment_closest(p, a, b, closest, t, distance2, ierr, diagnostics)
real(c_double), intent(in) :: p(3)
real(c_double), intent(in) :: a(3)
real(c_double), intent(in) :: b(3)
real(c_double), intent(out) :: closest(3)
real(c_double), intent(out) :: t
real(c_double), intent(out) :: distance2
integer, intent(out) :: ierr
type(CCL_Geometry_Diagnostics_Type), intent(inout), optional :: diagnostics
real(c_double) :: ab(3)
real(c_double) :: denom
ierr = CCL_SUCCESS
closest = 0.0_c_double
t = 0.0_c_double
distance2 = 0.0_c_double
if (.not. finite_vector3(p) .or. .not. finite_vector3(a) .or. .not. finite_vector3(b)) then
ierr = CCL_ERROR_INPUT_INVALID_VALUE
return
end if
ab = b - a
denom = ccl_vec3_dot(ab, ab)
if (denom <= CCL_GEOM_EPS * CCL_GEOM_EPS) then
ierr = CCL_ERROR_INPUT_INVALID_VALUE
if (present(diagnostics)) diagnostics%degenerate_edge_count = diagnostics%degenerate_edge_count + 1
return
end if
t = ccl_vec3_dot(p - a, ab) / denom
t = max(0.0_c_double, min(1.0_c_double, t))
closest = a + t * ab
distance2 = ccl_vec3_dot(p - closest, p - closest)
end subroutine ccl_point_segment_closest
subroutine ccl_point_triangle_closest(p, a, b, c, closest, weights, distance2, ierr, diagnostics)
real(c_double), intent(in) :: p(3)
real(c_double), intent(in) :: a(3)
real(c_double), intent(in) :: b(3)
real(c_double), intent(in) :: c(3)
real(c_double), intent(out) :: closest(3)
real(c_double), intent(out) :: weights(3)
real(c_double), intent(out) :: distance2
integer, intent(out) :: ierr
type(CCL_Geometry_Diagnostics_Type), intent(inout), optional :: diagnostics
real(c_double) :: ab(3), ac(3), ap(3), bp(3), cp(3), normal(3)
real(c_double) :: d1, d2, d3, d4, d5, d6
real(c_double) :: va, vb, vc, denom, v, w
ierr = CCL_SUCCESS
closest = 0.0_c_double
weights = 0.0_c_double
distance2 = 0.0_c_double
if (.not. finite_vector3(p) .or. .not. finite_vector3(a) .or. &
.not. finite_vector3(b) .or. .not. finite_vector3(c)) then
ierr = CCL_ERROR_INPUT_INVALID_VALUE
return
end if
ab = b - a
ac = c - a
call ccl_vec3_cross(ab, ac, normal)
if (ccl_vec3_dot(normal, normal) <= CCL_GEOM_EPS * CCL_GEOM_EPS) then
ierr = CCL_ERROR_INPUT_INVALID_VALUE
if (present(diagnostics)) diagnostics%degenerate_triangle_count = diagnostics%degenerate_triangle_count + 1
return
end if
ap = p - a
d1 = ccl_vec3_dot(ab, ap)
d2 = ccl_vec3_dot(ac, ap)
if (d1 <= 0.0_c_double .and. d2 <= 0.0_c_double) then
closest = a
weights = [1.0_c_double, 0.0_c_double, 0.0_c_double]
distance2 = ccl_vec3_dot(p - closest, p - closest)
return
end if
bp = p - b
d3 = ccl_vec3_dot(ab, bp)
d4 = ccl_vec3_dot(ac, bp)
if (d3 >= 0.0_c_double .and. d4 <= d3) then
closest = b
weights = [0.0_c_double, 1.0_c_double, 0.0_c_double]
distance2 = ccl_vec3_dot(p - closest, p - closest)
return
end if
vc = d1 * d4 - d3 * d2
if (vc <= 0.0_c_double .and. d1 >= 0.0_c_double .and. d3 <= 0.0_c_double) then
v = d1 / (d1 - d3)
closest = a + v * ab
weights = [1.0_c_double - v, v, 0.0_c_double]
distance2 = ccl_vec3_dot(p - closest, p - closest)
return
end if
cp = p - c
d5 = ccl_vec3_dot(ab, cp)
d6 = ccl_vec3_dot(ac, cp)
if (d6 >= 0.0_c_double .and. d5 <= d6) then
closest = c
weights = [0.0_c_double, 0.0_c_double, 1.0_c_double]
distance2 = ccl_vec3_dot(p - closest, p - closest)
return
end if
vb = d5 * d2 - d1 * d6
if (vb <= 0.0_c_double .and. d2 >= 0.0_c_double .and. d6 <= 0.0_c_double) then
w = d2 / (d2 - d6)
closest = a + w * ac
weights = [1.0_c_double - w, 0.0_c_double, w]
distance2 = ccl_vec3_dot(p - closest, p - closest)
return
end if
va = d3 * d6 - d5 * d4
if (va <= 0.0_c_double .and. (d4 - d3) >= 0.0_c_double .and. (d5 - d6) >= 0.0_c_double) then
w = (d4 - d3) / ((d4 - d3) + (d5 - d6))
closest = b + w * (c - b)
weights = [0.0_c_double, 1.0_c_double - w, w]
distance2 = ccl_vec3_dot(p - closest, p - closest)
return
end if
denom = 1.0_c_double / (va + vb + vc)
v = vb * denom
w = vc * denom
closest = a + ab * v + ac * w
weights = [1.0_c_double - v - w, v, w]
distance2 = ccl_vec3_dot(p - closest, p - closest)
end subroutine ccl_point_triangle_closest
subroutine ccl_triangle_barycentric(point, a, b, c, weights, ierr, diagnostics)
real(c_double), intent(in) :: point(3)
real(c_double), intent(in) :: a(3)
real(c_double), intent(in) :: b(3)
real(c_double), intent(in) :: c(3)
real(c_double), intent(out) :: weights(3)
integer, intent(out) :: ierr
type(CCL_Geometry_Diagnostics_Type), intent(inout), optional :: diagnostics
real(c_double) :: v0(3), v1(3), v2(3)
real(c_double) :: d00, d01, d11, d20, d21, denom
ierr = CCL_SUCCESS
weights = 0.0_c_double
v0 = b - a
v1 = c - a
v2 = point - a
d00 = ccl_vec3_dot(v0, v0)
d01 = ccl_vec3_dot(v0, v1)
d11 = ccl_vec3_dot(v1, v1)
d20 = ccl_vec3_dot(v2, v0)
d21 = ccl_vec3_dot(v2, v1)
denom = d00 * d11 - d01 * d01
if (.not. ieee_is_finite(denom) .or. abs(denom) <= CCL_GEOM_EPS * CCL_GEOM_EPS) then
ierr = CCL_ERROR_INPUT_INVALID_VALUE
if (present(diagnostics)) diagnostics%degenerate_triangle_count = diagnostics%degenerate_triangle_count + 1
return
end if
weights(2) = (d11 * d20 - d01 * d21) / denom
weights(3) = (d00 * d21 - d01 * d20) / denom
weights(1) = 1.0_c_double - weights(2) - weights(3)
end subroutine ccl_triangle_barycentric
subroutine ccl_point_quad_closest(p, x, closest, weights4, distance2, ierr, diagnostics)
real(c_double), intent(in) :: p(3)
real(c_double), intent(in) :: x(3, 4)
real(c_double), intent(out) :: closest(3)
real(c_double), intent(out) :: weights4(4)
real(c_double), intent(out) :: distance2
integer, intent(out) :: ierr
type(CCL_Geometry_Diagnostics_Type), intent(inout), optional :: diagnostics
real(c_double) :: closest_a(3), closest_b(3)
real(c_double) :: weights_a(3), weights_b(3)
real(c_double) :: distance2_a, distance2_b
integer :: ierr_a
integer :: ierr_b
closest = 0.0_c_double
weights4 = 0.0_c_double
distance2 = 0.0_c_double
call ccl_point_triangle_closest(p, x(:, 1), x(:, 2), x(:, 3), closest_a, weights_a, &
distance2_a, ierr_a, diagnostics)
call ccl_point_triangle_closest(p, x(:, 1), x(:, 3), x(:, 4), closest_b, weights_b, &
distance2_b, ierr_b, diagnostics)
if (ierr_a /= CCL_SUCCESS .and. ierr_b /= CCL_SUCCESS) then
ierr = CCL_ERROR_INPUT_INVALID_VALUE
return
end if
ierr = CCL_SUCCESS
if (ierr_b /= CCL_SUCCESS .or. (ierr_a == CCL_SUCCESS .and. distance2_a <= distance2_b)) then
closest = closest_a
distance2 = distance2_a
weights4 = [weights_a(1), weights_a(2), weights_a(3), 0.0_c_double]
else
closest = closest_b
distance2 = distance2_b
weights4 = [weights_b(1), 0.0_c_double, weights_b(2), weights_b(3)]
end if
end subroutine ccl_point_quad_closest
subroutine ccl_geometry_diagnostics_reset(diagnostics)
type(CCL_Geometry_Diagnostics_Type), intent(out) :: diagnostics
diagnostics%degenerate_edge_count = 0
diagnostics%degenerate_triangle_count = 0
diagnostics%invalid_normal_count = 0
end subroutine ccl_geometry_diagnostics_reset
logical function finite_vector3(value) result(is_finite)
real(c_double), intent(in) :: value(3)
is_finite = ieee_is_finite(value(1)) .and. ieee_is_finite(value(2)) .and. ieee_is_finite(value(3))
end function finite_vector3
end module ccl_geometry