2D Pitch Plot in UW-Space

#######################################################
# Plot of Pitch in UW-Space (With & Without Movement) #
#######################################################

#########
# FILES #
#########

source("Pitch_Plane_Functions.r");

########
# MAIN #
########

# PITCHf/x Data
PITCH <- list(ax = -8.2, ay = 29.2, az = -41.24, vx0 = -5.65, vy0 = -121.96, vz0 = 2.27, x0 = 2.6, y0 = 50.0, z0 = 5.35);

# Find an orthogonal matrix consisting of basis vectors for the plane transformation
BV <- PFX_Basis(PITCH);

# Find the two-angle form of the vector in spherical coordinates
rho <- Vertical_Angle(PITCH);

# Find the transpose of the orthogonal matrix
BV_T <- t(BV);

# Find the time to the plate
a <- 0.5*PITCH$ay;
b <- PITCH$vy0;
c_plate <- PITCH$y0 - (17/12);
t_plate <- (-b - sqrt(b^2 - 4*a*c_plate))/(2*a);

# Find the time at 55 feet (assumed release point)
c_55 <- PITCH$y0 - 55;
t_55 <- (-b - sqrt(b^2 - 4*a*c_55))/(2*a);

# Find the time at 4o feet (for in-plane movement calculation)
c_40 <- PITCH$y0 - 40;
t_40 <- (-b - sqrt(b^2 - 4*a*c_40))/(2*a);

# Set the number of intervals in the trajectory
N <- 100;

# Set the time increment
delta_t <- (t_plate-t_55)/N;

# Set up arrays to store the new coordinates
u_coord <- array(0,dim=(N+1));
w_coord <- array(0,dim=(N+1));
w_g_coord <- array(0,dim=(N+1));
w_no_mvt_coord <- array(0,dim=(N+1));
w_no_mvt_g_coord <- array(0,dim=(N+1));

# Transform the acceleration, velocity, and position
b0 <- BV_T[1,] %*% c(PITCH$x0,PITCH$y0,PITCH$z0);
u0 <- BV_T[2,] %*% c(PITCH$x0,PITCH$y0,PITCH$z0);
w0 <- BV_T[3,] %*% c(PITCH$x0,PITCH$y0,PITCH$z0);
vu0 <- BV_T[2,] %*% c(PITCH$vx0,PITCH$vy0,PITCH$vz0);
vw0 <- BV_T[3,] %*% c(PITCH$vx0,PITCH$vy0,PITCH$vz0);
au <- BV_T[2,] %*% c(PITCH$ax,PITCH$ay,PITCH$az);
aw <- BV_T[3,] %*% c(PITCH$ax,PITCH$ay,PITCH$az);

# Gravity
g <- c(0,0,-32.174);
g_basis <- BV_T %*% g;

# Find the position of the pitch in (u,w)-space when y = 40 feet
x_40 <- PITCH$x0 + PITCH$vx0*t_40 + 0.5*PITCH$ax*t_40^2;
y_40 <- PITCH$y0 + PITCH$vy0*t_40 + 0.5*PITCH$ay*t_40^2;
z_40 <- PITCH$z0 + PITCH$vz0*t_40 + 0.5*PITCH$az*t_40^2;
u_40 <- BV_T[2,] %*% c(x_40,y_40,z_40);
w_40 <- BV_T[3,] %*% c(x_40,y_40,z_40);

# Find the velocity when y = 40 feet
vx_40 <- PITCH$ax*t_40 + PITCH$vx0;
vy_40 <- PITCH$ay*t_40 + PITCH$vy0;
vz_40 <- PITCH$az*t_40 + PITCH$vz0;
vw_40 <- BV_T[3,] %*% c(vx_40,vy_40,vz_40);

# Form the pitch trajectory in (u,w)-space
for (i in 0:N){
    t_current <- t_55 + i*delta_t;
    u <- u0 + vu0*t_current + 0.5*au*t_current^2;
    w <- w0 + vw0*t_current + 0.5*aw*t_current^2;
    if (t_current < t_40){
        w_no_mvt <- w;
        w_no_mvt_g <- w;
    } else {
        w_no_mvt <- w_40 + vw_40*(t_current - t_40) + 0.5*g_basis[3]*(t_current-t_40)^2;
        w_no_mvt_g <- w_40 + vw_40*(t_current - t_40);
    }
    u_coord[i+1] <- u;
    w_coord[i+1] <- w;
    w_no_mvt_coord[i+1] <- w_no_mvt;
    w_no_mvt_g_coord[i+1] <- w_no_mvt_g;
}

# Set limits for the plot
u_min <- min(u_coord);
u_max <- max(u_coord);
w_min <- min(w_coord);
w_max <- max(w_coord);
w_no_mvt_min <- min(w_no_mvt_coord);
w_no_mvt_max <- max(w_no_mvt_coord);
w_no_mvt_g_min <- min(w_no_mvt_g_coord);
w_no_mvt_g_max <- max(w_no_mvt_g_coord);
w_all_min <- min(w_min,w_no_mvt_min,w_no_mvt_g_min);
w_all_max <- max(w_max,w_no_mvt_max,w_no_mvt_g_max);

K <- 0.01;
u_limits <- c(u_min - 0.01*abs(u_max-u_min),u_max + 0.01*abs(u_max-u_min));
w_limits <- c(w_all_min - 0.01*abs(w_all_max-w_all_min),w_all_max + 0.01*abs(w_all_max-w_all_min));

# Calculate the movement
mvt <- IP_Movement(PITCH);
mvt_g <- IP_Movement_G(PITCH);

# Plot
plot(u_coord,w_no_mvt_coord,pch=20,type='n',xlim=u_limits,ylim=w_limits,xlab="",ylab="",xaxs='i',yaxs='i',xaxt='n',yaxt='n');
lines(u_coord,w_no_mvt_coord,col='green',lwd=2);
par(new=TRUE);
plot(u_coord,w_no_mvt_g_coord,pch=20,type='n',xlim=u_limits,ylim=w_limits,xlab="",ylab="",xaxs='i',yaxs='i',xaxt='n',yaxt='n');
lines(u_coord,w_no_mvt_g_coord,col='blue',lwd=2);
par(new=TRUE);
plot(u_coord,w_coord,pch=20,type='n',xlim=u_limits,ylim=w_limits,xlab="u (feet)",ylab="w (feet)",xaxs='i',yaxs='i',main='Pitch in UW-Space');
lines(u_coord,w_coord,col='red',lwd=2);
par(new=TRUE);
plot(u_40,w_40,pch=20,col='black',xlim=u_limits,ylim=w_limits,xlab="",ylab="",xaxs='i',yaxs='i',xaxt='n',yaxt='n');
par(new=FALSE);
legend(0.99*u_limits[2],1.01*w_limits[1],c('In-Plane Pitch','Minus IPM','Minus IPM & Gravity'),col=c('red','green','blue'),lty=c(1,1,1),lwd=c(2,2,2),xjust=1,yjust=0);

# Round all values for display
rho <- round(rho,digits=3);
u0 <- round(u0,digits=3);
w0 <- round(w0,digits=3);
b0 <- round(b0,digits=3);
vu0 <- round(vu0,digits=3);
vw0 <- round(vw0,digits=3);
au <- round(au,digits=3);
aw <- round(aw,digits=3);
g_basis <- round(g_basis,digits=3);
mvt <- round(mvt,digits=3);
mvt_g <- round(mvt_g,digits=3);

# Print relevant values
print(paste("angle = ",rho,"",sep=""));
print(paste("u0 = ",u0,", w0 = ",w0,", b0 = ",b0,"",sep=""));
print(paste("vu0 = ",vu0,", vw0 = ",vw0,"",sep=""));
print(paste("au = ",au,", aw = ",aw,"",sep=""));
print(paste("gravity = (",g_basis[1],",",g_basis[2],",",g_basis[3],")",sep=""));
print(paste("movement = ",mvt," inches",sep=""));
print(paste("movement + gravity = ",mvt_g," inches",sep=""));