# distance between 2 points
eucl.dist <- function(x1, x2) sqrt(sum((x1 - x2) ^ 2))
# area of triangle
area <- function(x1,x2,x3) {
v1 = x1 - x2
v2 = x1 - x3
0.5* sqrt( (v1[2]*v2[3] - v1[3]*v2[2])**2 +
(v1[3]*v2[1] - v1[1]*v2[3])**2 +
(v1[1]*v2[2] - v1[2]*v2[1])**2)
}
# function to calculate curvature (optional for this problem)
curvature <- function(x1, x2, x3){
4 * area(x1, x2, x3) / (eucl.dist(x1,x2)*eucl.dist(x2,x3)*eucl.dist(x3,x1))
}
# function to calculate radius
radius <- function(x1, x2, x3){
0.25 * (eucl.dist(x1,x2)*eucl.dist(x2,x3)*eucl.dist(x3,x1)) / area(x1, x2, x3)
}
# Test on some simple 2D case
path2D <- cbind( c(0, 1, 2, 2, 3, 4, 4, 4),
c(0, 1, 0,-1,-1, 0, 1, 3),
rep(0,8) )
names(path2D) <- x("x","y","z")
#plot the path
plot( path2D[,1], path2D[,2], type="b",
lwd=2, col="slateblue",
xlim=c(-1,5), ylim=c(-2,4),
xlab="x", ylab="y")
# calculate vector with radii
rads <- sapply( 1 : (nrow(path2D) - 2), FUN=function(i){ radius(x1 = path2D[i,], x2 = path2D[i+1,], x3 = path2D[i+2,] )} )
# [1] 1.0000000 1.5811388 0.7071068 1.5811388 1.5811388 Inf
# Add text to the plot
text(path2D[2,1],path2D[2,2], round(rads[1],2), pos=3, cex=.8)
text(path2D[3,1],path2D[3,2], round(rads[2],2), pos=4, cex=.8)
text(path2D[4,1],path2D[4,2], round(rads[3],2), pos=2, cex=.8)
text(path2D[5,1],path2D[5,2], round(rads[4],2), pos=1, cex=.8)
text(path2D[6,1],path2D[6,2], round(rads[5],2), pos=4, cex=.8)
text(path2D[7,1],path2D[7,2], round(rads[6],2), pos=4, cex=.8)
