Add CCLite 0.2.3 geometry primitives
This commit is contained in:
@@ -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
|
||||
Reference in New Issue
Block a user