copyTransformation <- function(m1, m2, mn, translate = TRUE, lar.cons = NULL, lar.compare = NULL){
## Written to work with point sets that are identical except for translation/rotation
#if(!identical.sets){
#}
# IF ONLY ONE POINT PROVIDED ONLY APPLY TRANSLATION
if(!is.matrix(m1)){
if(is.matrix(mn)){
return(mn + matrix(m2 - m1, nrow=nrow(mn), ncol=ncol(mn), byrow=TRUE))
}else{
return(mn + (m2 - m1))
}
}else{
if(nrow(m1) != nrow(m2)) stop(paste0("'m1' ", nrow(m1), " must have the same number of rows as 'm2' ", nrow(m2), "."))
}
# CONVERT TO MATRIX IF VECTOR
if(is.vector(mn)) mn <- matrix(mn, 1, 3)
# REMOVE NA VALUES IN EITHER
is_na_m1 <- is.na(m1[, 1])
is_na_m2 <- is.na(m2[, 1])
is_na_m <- is_na_m1+is_na_m2 > 0
m1 <- m1[!is_na_m, ]
m2 <- m2[!is_na_m, ]
## RETURN NA MATRIX IF 0 OR 1 ROW(S)
if(is.null(nrow(m1))) return(mn*NA)
if(nrow(m1) < 2) return(mn*NA)
## IF AT LEAST TWO POINTS IN M1 ARE THE SAME, ONLY APPLY TRANSLATION
if(sum(distPointToPoint(m1, m1[c(2:nrow(m1),1), ]) == 0) > 0)
return(mn + matrix(m2[1, ] - m1[1, ], nrow=nrow(mn), ncol=ncol(mn), byrow=TRUE))
## EMPTY RETURN MATRIX
if(nrow(mn) == 0) return(mn)
# TRANSFORMED POINT MATRIX
mr <- matrix(NA, nrow=nrow(mn), ncol=ncol(mn))
## CHECK WHETHER SIMPLE TRANSLATION COPIES TRANSFORMATION
if(sum(apply(m2-m1, 2, 'sd')) < 1e-10){
return(mn + matrix((m2-m1)[1, ], nrow=nrow(mn), ncol=3, byrow=TRUE))
}
# GET NON COLLINEAR GROUP OF POINTS
if(nrow(m1) > 2){
# REMOVE ANY POINTS THAT ARE COLLINEAR WITH FIRST TWO
retain <- rep(TRUE, nrow(m1))
for(i in 3:nrow(m1)){
#if(distPointToLine(pt=m2[i, ], l1=m1[1, ], l2=m1[2, ]) > 1e-10) retain[i] <- FALSE # Changed this - not sure what error it was intended to prevent
if(distPointToLine(pt=m1[i, ], l1=m1[1, ], l2=m1[2, ]) < 1e-10) retain[i] <- FALSE
}
m1 <- m1[retain, ]
m2 <- m2[retain, ]
}
# TRANSFORMATION GIVEN ONLY TWO POINTS (MINIMIZE ROTATION ABOUT LONG AXIS)
two_ref_pts <- ifelse(nrow(m1) == 2, TRUE, FALSE)
if(two_ref_pts){
m1 <- rbind(m1, m1[1, ] + uvector(vorthogonal(m1[2, ] - m1[1, ])))
m2 <- rbind(m2, m2[1, ] + uvector(vorthogonal(m2[2, ] - m2[1, ])))
}
# INCLUDE ORIGIN
if(!translate) mn <- rbind(mn, c(0,0,0))
# FIND INITIAL COORDINATE SYSTEM AXES
rm_i <- setCoordinateAxes(m1)
# FIND INITIAL POSITION VECTOR MATRIX
mn_ref <- (mn - matrix(m1[1, ], nrow=nrow(mn), ncol=3, byrow=TRUE)) %*% t(rm_i)
# FIND FINAL COORDINATE SYSTEM AXES
ca <- setCoordinateAxes(m2)
# FIND ROTATION MATRIX TO TRANSFORM TO FINAL COORDINATE SYSTEM
rm <- tMatrixDC(ca)
# TRANSFORM POINTS
mr <- (mn_ref %*% t(rm)) + matrix(m2[1, ], nrow=nrow(mn_ref), ncol=3, byrow=TRUE)
# GET ALIGNMENT ERROR AFTER UNIFICATION
#align_error <- sum(abs(round(m2 - mr[rownames(mr) %in% rownames(m2), ], 6)))
# CENTER BY PREVIOUS ORIGIN
if(!translate){
mr <- mr - matrix(mr[nrow(mr), ], nrow=nrow(mr), ncol=3, byrow=TRUE)
mr <- mr[1:(nrow(mr)-1), ]
}
if(two_ref_pts){
if(!is.null(lar.cons)){
# TRANSFORM REFERENCE POINT
pt_ref <- (((lar.cons$point - m1[1, ]) %*% t(rm_i)) %*% t(rm)) + m2[1, ]
# FIND ROTATION ABOUT LONG AXIS THAT KEEPS CONSTRAINT
# DEFINE CIRCLE USING POINT FOR RADIUS AND TWO JOINTS AS LONG-AXIS
circle <- defineCircle(center=m2[1, ], nvector=m2[1, ]-m2[2, ], point_on_radius=pt_ref)
# FIND INTERSECTION OF CIRCLE AND PLANE
icp_t <- intersectCirclePlane(circle, P=lar.cons$point.i, N=lar.cons$vec)
# ADD NEGATIVE ANGLES
icp_t <- c(icp_t, -icp_t)
# FIND THE DIRECTION IN WHICH TO ROTATE POINTS
ref_rot <- matrix(NA, nrow=length(icp_t), ncol=3)
for(i in 1:length(icp_t)){
ref_rot[i, ] <- (pt_ref - m2[1, ]) %*% tMatrixEP(m2[1, ]-m2[2, ], a=icp_t[i]) + m2[1, ]
}
dist_plane <- abs(distPointToPlane(ref_rot, n=lar.cons$vec, q=lar.cons$point.i))
# KEEP PROPER ROTATION ANGLES
icp_t <- icp_t[dist_plane < 1e-9]
if(length(icp_t) > 1){
# ROTATE POINTS OVER ANGLES
ref_rot <- matrix(NA, nrow=length(icp_t), ncol=3)
for(i in 1:length(icp_t)) ref_rot[i, ] <- (pt_ref - m2[1, ]) %*% tMatrixEP(m2[1, ]-m2[2, ], a=icp_t[i]) + m2[1, ]
# FIND THE ANGLE THAT PUTS POINT CLOSEST TO THE PREVIOUS POINT
icp_t <- icp_t[which.min(distPointToPoint(ref_rot, lar.compare))]
}
# CENTER POINTS ABOUT ONE OF POINTS ON AXIS OF ROTATION
mr_centroid_m <- matrix(m2[1, ], nrow=nrow(mr), ncol=3, byrow=TRUE)
mrc <- mr - mr_centroid_m
# APPLY ROTATION
mrr <- mrc %*% tMatrixEP(m2[1, ]-m2[2, ], a=icp_t)
# UNDO CENTERING TRANSLATION
mrr <- mrr + mr_centroid_m
return(mrr)
}
# FIND A POINT NOT COINCIDENT WITH END POINTS
ref_idx <- NA
for(i in 1:nrow(mn)){
if(is.na(mn[i, 1])) next
if(distPointToLine(mn[i, ], m1[1, ], m1[2, ]) > 1e-13){ref_idx <- i;break}
}
# IF ALL POINTS ARE COINCIDENT WITH END POINTS RETURN TRANSFORMED POINTS
if(is.na(ref_idx)) return(mr)
# FIND POINT ON LINE BETWEEN END POINTS, AT NORMAL TO REF POINT
ref_norm <- pointNormalOnLine(mn[ref_idx, ], m1[1, ], m1[2, ])
#print(distPointToLine(ref_norm, m1[1, ], m1[2, ]))
# GET DISTANCE FROM LINE TO REF POINT
ref_norm_dist <- distPointToPoint(mn[ref_idx, ], ref_norm)
# TRANSFORM REF NORMAL USING SAME TRANSFORMATION AS TO MN
ref_norm_t <- (((ref_norm - m1[1, ]) %*% t(rm_i)) %*% t(rm)) + m2[1, ]
# VECTOR FROM REF NORM TO REF POINT
ref_vector <- uvector(mn[ref_idx, ] - ref_norm)*ref_norm_dist
# TRANSLATED REF POINT FROM REF NORM
ref_toproj <- ref_proj <- ref_norm_t + ref_vector
# FIND MINIMUM DISTANCE BETWEEN POINT AND TRANSFORMED POINT POTENTIAL LOCATIONS
minDist <- distPointToPlane(p=ref_toproj, n=uvector(m2[2, ] - m2[1, ]), q=ref_norm_t)
# PROJECT TRANSLATED REF POINT INTO PLANE
if(minDist > 1e-10){
ref_proj <- pointPlaneProj(q=ref_toproj, p=ref_norm_t, n=uvector(m2[2, ] - m2[1, ]))
#ref_proj <- ref_norm_t + uvector(ref_proj - ref_norm_t)*ref_norm_dist
}
# FIND ANGLE BETWEEN TRANSFORMED POINT AND PROJECTED POINT
a <- avec(mr[ref_idx, ] - ref_norm_t, ref_proj - ref_norm_t)
# IF DIFFERENCE IN ANGLE IS 0, RETURN TRANSFORMED MATRIX AS IS
if(is.na(a) || a == 0) return(mr)
# CENTER POINTS ABOUT ONE OF POINTS ON AXIS OF ROTATION
mr_centroid_m <- matrix(m2[2, ], nrow=nrow(mr), ncol=3, byrow=TRUE)
mrc <- mr - mr_centroid_m
# APPLY ROTATIONS IN BOTH DIRECTIONS
mrc_p <- mrc %*% tMatrixEP(m2[2, ]-m2[1, ], a=a)
mrc_n <- mrc %*% tMatrixEP(m2[2, ]-m2[1, ], a=-a)
# UNDO CENTERING TRANSLATION
mrc_p <- mrc_p + mr_centroid_m
mrc_n <- mrc_n + mr_centroid_m
# IDENTIFY CORRECT DIRECTION OF ROTATION AND RETURN CORRESPONDING MATRIX
ifelse(distPointToPoint(ref_proj, mrc_p[ref_idx, ]) < distPointToPoint(ref_proj, mrc_n[ref_idx, ]), return(mrc_p), return(mrc_n))
}
mr
}
Add the following code to your website.
For more information on customizing the embed code, read Embedding Snippets.