solveJointPath <- function(joint.types, joint.status, joint.coor, joint.cons, body.num,
input, body.conn, joint.names, joint.prev, joint.ref, iter = 1, print.progress = FALSE,
indent = '', indent.level = 4){
# INPUT
if(length(joint.types) == 1){
# SET NULL TRANSFORMATION MATRICES
tmat1 <- tmat2 <- tmat3 <- tmat4 <- diag(4)
# GET JOINT SET TO TRANSFORM
jt_set <- which(body.num == body.conn)
# Logical if input transformation set
tform_set <- FALSE
if(print.progress) joint_props <- c()
# CREATE TRANSFORMATION MATRIX
if(joint.types %in% c('S', 'X', 'XO', 'R', 'U', 'PR', 'N', 'O')){
# Set rotation magnitudes
if(joint.types == 'N'){
mags <- input[iter, 4:6]
}else if(joint.types == 'PR'){
mags <- input[iter, 3]
}else{
mags <- input[iter, ]
}
# SKIP IF INPUTS ARE NA (ALLOWS INPUT RESOLVE PARAMETERS TO BE ADDED IN ADDITION TO INPUT PARAMETERS)
if(sum(is.na(mags)) == length(mags)) return(list('body.tmat'=NULL,'joint.status'=NULL,'solution'=TRUE))
# Set axes of rotation
if(joint.types == 'PR'){
AOR <- array(cprod(joint.cons[[1]][1, , iter, jt_set], joint.cons[[1]][2, , iter, jt_set]), dim=c(1,3,1))
}else{
AOR <- array(joint.cons[[1]][, , iter, ], dim=dim(joint.cons[[1]])[c(1,2,4)])
}
#
if(joint.types %in% c('U', 'O')){
jt_set <- 1:2
# Apply first axis rotation to second axis of U-joint and second and third axis of V-joint
AOR[2:dim(AOR)[1], , jt_set[2]] <- rotateBody(m=AOR[2:dim(AOR)[1], , jt_set[2]], v=AOR[1, , jt_set[1]], a=mags[1])
# Apply second axis rotation to third axis of V-joint
if(joint.types == 'O') AOR[3, , jt_set[2]] <- rotateBody(m=AOR[3, , jt_set[2]], v=AOR[2, , jt_set[2]], a=mags[2])
}else{
jt_set <- rep(jt_set, dim(AOR)[1])
}
if(print.progress){
AORs_print <- c()
for(i in 1:dim(AOR)[1]){
if(i == 1){ jt_set_i <- 1 }else{ jt_set_i <- 2 }
AORs_print <- c(AORs_print, paste0(round(AOR[i, , jt_set_i], 3), collapse=','))
}
AOR_print <- paste0(paste0('AoR', 1:length(AORs_print), ':', AORs_print), collapse='; ')
joint_props <- c(joint_props, paste0('CoR:', paste0(round(joint.coor[, jt_set[1]], 3), collapse=',')))
joint_props <- c(joint_props, AOR_print)
joint_props <- c(joint_props, paste0('Angle:', paste0(round(mags, 3), collapse=',')))
}
# TRANSLATE TO CENTER OF ROTATION (JOINT)
tmat1[1:3, 4] <- joint.coor[, jt_set[1]]
# LOOP THROUGH EACH COLUMNN OF INPUT PARAMETERS
for(i in dim(AOR)[1]:1){
# SKIP IF NA
if(is.na(mags[i])) next
# Set joint set
if(i == 1){ jt_set_i <- 1 }else{ jt_set_i <- 2 }
# APPLY ROTATION ABOUT SINGLE JOINT CONSTRAINT VECTOR
tmat2[1:3, 1:3] <- tmat2[1:3, 1:3] %*% tMatrixEP(AOR[i, , jt_set_i], -mags[i])
}
# TRANSLATE BACK FROM CENTER OF ROTATION
tmat3[1:3, 4] <- -joint.coor[, jt_set[1]]
# Transform found
tform_set <- TRUE
}
if(joint.types %in% c('L', 'P', 'T', 'PR', 'N')){
# Set translation magnitudes
if(joint.types == 'N'){
mags <- input[iter, 1:3]
}else if(joint.types == 'PR'){
mags <- input[iter, 1:2]
}else{
mags <- input[iter, ]
}
if(print.progress){
tvecs_print <- c()
for(i in 1:dim(joint.cons[[1]])[1]) tvecs_print <- c(tvecs_print, paste0(round(joint.cons[[1]][i, , iter, jt_set], 3), collapse=','))
tvec_print <- paste0(paste0('Tvec', 1:length(tvecs_print), ':', tvecs_print), collapse='; ')
joint_props <- c(joint_props, tvec_print)
joint_props <- c(joint_props, paste0('Mag:', paste0(round(mags, 3), collapse=',')))
}
# TRANSLATE
tmat4[1:3, 4] <- colSums(mags*matrix(joint.cons[[1]][, , iter, jt_set], ncol=3))
# Transform found
tform_set <- TRUE
}
# Error if joint not found
if(!tform_set){cat('\n');stop(paste0("Unrecognized joint type '", joint.types, "'"))}
if(print.progress) cat(paste0('{', paste0(joint_props, collapse='; '), '}\n'))
# CHANGE JOINT STATUS
joint.status[unique(jt_set)] <- 'i'
# COMBINE TRANSFORMATION MATRICES
tmat <- tmat1 %*% tmat2 %*% tmat3 %*% tmat4
return(list('body.tmat'=list(tmat), 'joint.status'=joint.status))
# SOLVE
}else{
tmat_body1 <- NULL
# SET TRANSFORM INDICATOR
trfm_vec <- rep('', length(joint.types))
#trfm_vec[rowSums(joint.status == '') > 0] <- '^'
trfm_vec[rowSums(joint.status == 'c') == 2] <- '*'
trfm_vec[rowSums((joint.status == 'f')+(joint.status == 'c')) == 2] <- '*'
trfm_vec[rowSums((joint.status == '')+(joint.status == 'c')) == 2] <- '*'
# trfm_vec[rowSums((joint.status == '')+(joint.status == 'c')) == 2] <- '*'
# CREATE STRING WITH JOINT TYPES AND TRANSFORM INDICATOR
type_str <- paste0(paste0(joint.types, trfm_vec), collapse='-')
# REVERSE INPUTS
if(type_str %in% c('S-S*-L', 'S*-S-S', 'S*-S-S-S', 'R-S*-S')){
solve_joint_path <- solveJointPath(
joint.types=joint.types[length(joint.types):1],
joint.status=joint.status[nrow(joint.status):1, ],
joint.coor=joint.coor[nrow(joint.coor):1, , ],
joint.cons=joint.cons[length(joint.cons):1],
body.num=body.num[length(body.num):1],
input=input[length(input):1],
body.conn=body.conn,
joint.names=joint.names[length(joint.cons):1],
joint.prev=joint.prev[nrow(joint.prev):1, ],
joint.ref=joint.ref[nrow(joint.ref):1, ],
iter=iter, print.progress=print.progress, indent=indent, indent.level=indent.level)
if(is.null(solve_joint_path$body.tmat)) return(list('body.tmat'=NULL,'joint.status'=NULL,'solution'=solve_joint_path$solution))
return(list(
'body.tmat'=solve_joint_path$body.tmat[length(solve_joint_path$body.tmat):1],
'joint.status'=solve_joint_path$joint.status[nrow(solve_joint_path$joint.status):1, ],
'solution'=solve_joint_path$solution
))
}
if(type_str == 'S-R*-S'){
# NEED INPUT RESOLVE
}
# IDENTIFY J2 SET TRANSFORMED WITH JOINT 1 AND 3
J2_J1_set <- which(body.conn[2, ] != body.num[2])
J2_J3_set <- which(body.conn[2, ] == body.num[2])
# PRINT PATH DETAILS
if(print.progress){
name_vec <- joint.names
name_wt <- paste0(paste0(name_vec, trfm_vec), collapse='-')
#cat(paste0(type_str, ' '))
# cat(paste0('Solve path (', name_wt, ')'))
cat(paste0(paste0(rep(indent, indent.level), collapse=''), 'Solving path ', name_wt, ''))
}
if(type_str %in% c('L-S*-S', 'S-S*-S')){
# GET SPHERE RADIUS BY MEASURING DISTANCE BETWEEN JOINTS
J3_sphere_r <- distPointToPoint(joint.coor[3, , 1], joint.coor[2, , J2_J3_set])
}
if(type_str %in% c('R-R*-R', 'S-S*-R', 'S-S*-S')){
# GET POINT DISTANCE
dist_J1_J2 <- distPointToPoint(joint.coor[2, , J2_J1_set], joint.coor[1, , 1])
}
if(type_str == 'S-S*-S'){
if(print.progress) cat(' by circle on sphere from point')
#if(print.progress) print(input)
# SET NULL TRANSFORMATION MATRICES
tmat4 <- tmat5 <- tmat6 <- tmat7 <- diag(4)
# DEFINE SPHERE OF POTENTIAL SOLUTIONS FROM J1
sphere <- list('C'=joint.coor[3, , 1], 'R'=J3_sphere_r)
# FIND CIRCLE ON SPHERE
circle <- circleOnSphereFromPoint(sphere, d=dist_J1_J2, p=joint.coor[1, , 1])
# IF NO CIRCLE
if(is.null(circle)) return(list('body.tmat'=NULL,'joint.status'=NULL,'solution'=TRUE))
# CHOOSE POINT ON CIRCLE
J2_npos <- circlePoint(circle, T=input[[2]][iter, 1])
## FIND ROTATION OF SECOND BODY
# ROTATION TO ALIGN WITH NEW JOINT POSITION
v_pre <- joint.coor[2, , J2_J3_set] - joint.coor[3, , 1]
v_new <- J2_npos - joint.coor[3, , 1]
# TRANSLATE BACK FROM CENTER OF ROTATION
tmat4[1:3, 4] <- joint.coor[3, , 1]
# ROTATION TO ALIGN WITH NEW JOINT POSITION
tmat6[1:3, 1:3] <- tMatrixEP(cprod(v_pre, v_new), -avec(v_pre, v_new))
# CHECK FOR ROTATION BETWEEN S-JOINTS
#if(!is.na(input[[2]][1]) || !is.na(input[[3]][1])){
# if(!is.na(input[[2]][1])) angle <- input[[2]][iter, 1]
# if(!is.na(input[[3]][1])) angle <- input[[3]][iter, 1]
# tmat5[1:3, 1:3] <- tMatrixEP(J2_npos - joint.coor[3, , 1], angle)
#}
if(!is.na(input[[3]][1])) tmat5[1:3, 1:3] <- tMatrixEP(J2_npos - joint.coor[3, , 1], angle <- input[[3]][iter, 1])
# TRANSLATE TO CENTER OF ROTATION
tmat7[1:3, 4] <- -joint.coor[3, , 1]
# COMBINE TRANSFORMATIONS
tmat_body2 <- tmat4 %*% tmat5 %*% tmat6 %*% tmat7
}
if(type_str %in% c('S-S*-R', 'R-R*-R')){
if(print.progress) cat(' by point on circle at distance from point')
## FIND JOINT POSITION THAT SOLVES CONSTRAINT
# DEFINE CIRCLE FOR OUTPUT LINK (NEED TO REDEFINE CENTER BECAUSE CIRCLE CENTER MAY NOT BE SAME AS JOINT)
output_circle <- defineCircle(center=joint.coor[3, , 1], nvector=joint.cons[[3]][, , iter, 1],
point_on_radius=joint.coor[2, , J2_J3_set], redefine_center=TRUE)
# FIND ANGLE ON CIRCLE AT DISTANCE FROM TRANSMISSION LINK JOINT
output_link_t <- angleOnCircleFromPoint(circle=output_circle, dist=dist_J1_J2,
P=joint.coor[1, , 1], point_compare=joint.prev[2, ])
# FIND CORRESPONDING POINT ON CIRCLE
J2_npos <- circlePoint(circle=output_circle, T=output_link_t)
# If no solution, return NULL
if(is.vector(J2_npos)){
if(is.na(J2_npos[1])) return(list('body.tmat'=NULL,'joint.status'=NULL,'solution'=FALSE))
}else{
if(is.na(J2_npos[1,1])) return(list('body.tmat'=NULL,'joint.status'=NULL,'solution'=FALSE))
}
}
if(type_str %in% c('S-S*-R', 'S-S*-S')){
# SET NULL TRANSFORMATION MATRICES
tmat1 <- tmat2 <- tmat3 <- tmat4 <- diag(4)
## FIND TRANSFORMATION OF FIRST BODY
# ROTATE COUPLER LINK TO MATCH VECTOR BETWEEN UNTRANSFORMED AND TRANSFORMED JOINT
v_pre <- joint.coor[2, , J2_J1_set] - joint.coor[1, , 1]
v_new <- J2_npos - joint.coor[1, , 1]
# TRANSLATE BACK FROM CENTER OF ROTATION
tmat1[1:3, 4] <- joint.coor[1, , 1]
# ROTATION TO ALIGN WITH NEW JOINT POSITION
tmat3[1:3, 1:3] <- tMatrixEP(cprod(v_pre, v_new), -avec(v_pre, v_new))
# CHECK FOR ROTATION BETWEEN S-JOINTS
#if(!is.na(input[[1]][1]) || !is.na(input[[2]][1])){
# if(!is.na(input[[1]][1])) angle <- input[[1]][iter, 1]
# if(!is.na(input[[2]][1])) angle <- input[[2]][iter, 1]
# tmat2[1:3, 1:3] <- tMatrixEP(J2_npos - joint.coor[1, , 1], angle)
#}
if(!is.na(input[[1]][1])) tmat2[1:3, 1:3] <- tMatrixEP(J2_npos - joint.coor[1, , 1], input[[1]][iter, 1])
# TRANSLATE TO CENTER OF ROTATION
tmat4[1:3, 4] <- -joint.coor[1, , 1]
# COMBINE TRANSFORMATIONS
tmat_body1 <- tmat1 %*% tmat2 %*% tmat3 %*% tmat4
}
if(type_str %in% c('S-S*-R', 'R-R*-R')){
# SET NULL TRANSFORMATION MATRICES
tmat4 <- tmat5 <- tmat6 <- diag(4)
## FIND ROTATION OF SECOND BODY
# ROTATION TO ALIGN WITH NEW JOINT POSITION
v_pre <- joint.coor[2, , J2_J3_set] - joint.coor[3, , 1]
v_new <- J2_npos - joint.coor[3, , 1]
# TRANSLATE BACK FROM CENTER OF ROTATION
tmat4[1:3, 4] <- joint.coor[3, , 1]
# ROTATION TO ALIGN WITH NEW JOINT POSITION
tmat5[1:3, 1:3] <- tMatrixEP(joint.cons[[3]][, , iter, 1], avec(v_pre, v_new, axis=joint.cons[[3]][, , iter, 1], about.axis=TRUE))
# TRANSLATE TO CENTER OF ROTATION
tmat6[1:3, 4] <- -joint.coor[3, , 1]
# COMBINE TRANSFORMATIONS
tmat_body2 <- tmat4 %*% tmat5 %*% tmat6
}
if(type_str == 'R-R*-R'){
# SET NULL TRANSFORMATION MATRICES
tmat1 <- tmat2 <- tmat3 <- diag(4)
## FIND ROTATION OF FIRST BODY
v_pre <- joint.coor[2, , J2_J1_set] - joint.coor[1, , 1]
v_new <- J2_npos - joint.coor[1, , 1]
# TRANSLATE BACK FROM CENTER OF ROTATION
tmat1[1:3, 4] <- joint.coor[1, , 1]
# ROTATION TO ALIGN WITH NEW JOINT POSITION
tmat2[1:3, 1:3] <- tMatrixEP(joint.cons[[1]][, , iter, 1], avec(v_pre, v_new, axis=joint.cons[[1]][, , iter, 1], about.axis=TRUE))
# TRANSLATE TO CENTER OF ROTATION
tmat3[1:3, 4] <- -joint.coor[1, , 1]
# COMBINE TRANSFORMATIONS
tmat_body1 <- tmat1 %*% tmat2 %*% tmat3
}
if(type_str == 'L-S*-S'){
if(print.progress) cat(' by intersection of sphere and line')
# SET NULL TRANSFORMATION MATRICES
tmat1 <- tmat2 <- tmat3 <- tmat4 <- diag(4)
#if(print.progress) print(joint.status)
## SLIDE S TO ALIGN L- AND S-JOINTS: INTERSECTION OF SPHERE AND LINE
# FIND INTERSECTION OF SPHERE AND LINE
J2_new <- intersectSphereLine(c=joint.coor[3, , 1],
r=J3_sphere_r, x=joint.coor[2, , J2_J1_set], l=joint.cons[[1]][, , iter, 1],
point.compare=joint.prev[2, ])
# FIND TRANSLATION ALONG L-JOINT CONSTRAINT VECTOR
tmat1[1:3, 4] <- J2_new - joint.coor[2, , J2_J1_set]
# BODY1 TRANSFORMATION
tmat_body1 <- tmat1
# TRANSLATE J3 TO ORIGIN
tmat2[1:3, 4] <- -joint.coor[3, , 1]
# FIND ROTATION
v_pre <- joint.coor[2, , 1] - joint.coor[3, , 1]
v_new <- J2_new - joint.coor[3, , 1]
rm_aor <- cprod(v_pre, v_new)
rm_mag <- avec(v_pre, v_new)
# FIND ROTATION ABOUT J3
tmat3[1:3, 1:3] <- tMatrixEP(rm_aor, -rm_mag)
# TRANSLATE J2 TO NEW POSITION
tmat4[1:3, 4] <- joint.coor[3, , 1]
# FIND TRANSFORMATION OF BODY2
tmat_body2 <- tmat4 %*% tmat3 %*% tmat2
}
if(type_str %in% c('L-S*-S', 'S-S*-R', 'S-S*-S', 'R-R*-R')){
# UPDATE JOINT STATUS
joint.status[2, ] <- 's'
}
if(type_str == 'S-S-S-S*'){
# IDENFITY S-JOINT ATTACHED TO SECOND BODY (BRIDGE JOINT)
# TRANSFORM FIRST BODY ABOUT NON-BRIDGE JOINTS SO THAT BRIDGE JOINT IS AT
# DISTANCE EQUAL TO LENGTH OF SECOND BODY
# ALSO ROTATE SECOND BODY ABOUT BRIDGE JOINT TOO
}
if(print.progress){
cat('\n')
}
if(!is.null(tmat_body1)){
return(list('body.tmat'=list(tmat_body1, tmat_body2), 'joint.status'=joint.status))
}else{
return(list('body.tmat'=NULL,'joint.status'=NULL,'solution'=FALSE))
}
}
}
Add the following code to your website.
For more information on customizing the embed code, read Embedding Snippets.