For an assignment I need to calculate the angle beta for the launch of a rocket.
This rocket should hit a skeet in the air and the rocket is just accelerating in the first 0,5s.
In my first part of the exercise I already calculated the trajectory of the skeet. Next to the given parameters there is a wind in x-direction as well.
I am trying to calculate the angle beta by minimizing a wrapper function for the distance of both trajectories.
I always get the TypeError: loop of ufunc does not support argument 0 of type int which has no callable sqrt method
I dont know what is my mistake and i hope someone can help me.
The experiment is looking like this:
Picture - rocket - skeet
from scipy.optimize import minimize
from scipy.optimize import fsolve
from scipy.integrate import solve_ivp
import numpy as np
import matplotlib.pyplot as plt
v_wind = 3.5 #m / s in positive x direction
rho_l = 1.2 # kg/m3
# skeet params
cw_skeet_top = 1.11
cw_skeet_side = 0.5
A_skeet_top = 0.0086
A_skeet_side = 0.00432
m_skeet = 0.105 #kg
v0_skeet = 40
x0_skeet = 60
y0_skeet = 0
# rocket params
cw_rocket_side = 1
cw_rocket_front = 0.5
A_rocket_side = 0.08
A_rocket_front = 0.0312
m_rocket = 2
v0_rocket = 0
x0_rocket = 20
y0_rocket = 0
F_accel_rocket = 120 #in Newton, divide by m_rocket to find accel in m/s^2
t_accel_end = 0.5
def drag(v_rel, rho_l, cw, A):
Fw = rho_l * cw * A * v_rel**2/2.0
if (v_rel < 0):
Fw = -Fw
return Fw
def skeet(t,y,rho_l,cw_skeet_side, cw_skeet_top,A_skeet_side,A_skeet_top,m_skeet):
# Zur Übersichtlichkeit:
# y[0] - x-position
# y[1] - x-velocity
# y[2] - y-position
# y[3] - y-velocity
d2xdt2 = -drag(y[1]-v_wind, rho_l, cw_skeet_side , A_skeet_side)/m_skeet
dxdt = -y[1]
d2ydt2 = -9.81 - drag(y[3], rho_l, cw_skeet_top, A_skeet_top)/m_skeet
dydt = y[3]
return dxdt, d2xdt2, dydt, d2ydt2
def hit_ground(t,y, *args):
return y[2]
hit_ground.terminal = True
hit_ground.direction = -1
final_x = 0 #m
DGLargs=(rho_l,cw_skeet_side, cw_skeet_top,A_skeet_side,A_skeet_top,m_skeet)
def wrapperAlpha(alpha,final_x,DGLargs):
tspan = [0, 100]
maxStep=tspan[1]/1000
start_vec = [x0_skeet, v0_skeet*np.cos(alpha/180*np.pi), y0_skeet, v0_skeet*np.sin(alpha/180*np.pi)]
sol = solve_ivp(skeet,tspan,start_vec,args=(DGLargs),max_step=maxStep,events=(hit_ground))
return np.abs(sol.y[0,-1] - final_x)
starting_guess = 10 #deg
res = fsolve(wrapperAlpha, x0 = starting_guess, args = (final_x,DGLargs))
angle = res
print(angle)
tspan = [0, 100]
maxStep=tspan[1]/1000
start_vec = [x0_skeet, v0_skeet*np.cos(angle/180*np.pi), y0_skeet, v0_skeet*np.sin(angle/180*np.pi)]
sol = solve_ivp(skeet,tspan,start_vec,args=(DGLargs),max_step=maxStep,events=(hit_ground))
x_skeet = sol.y[0,:]
y_skeet = sol.y[2,:]
plt.plot(x_skeet,y_skeet)
#important exercise:
RocketArgs = (rho_l,cw_rocket_side, cw_rocket_front,A_rocket_side,A_rocket_front,m_rocket,F_accel_rocket,t_accel_end)
def rocket(t,y,beta, rho_l,cw_rocket_side, cw_rocket_front,A_rocket_side,A_rocket_front,m_rocket,F_accel_rocket,t_accel_end):
if t_accel_end > t:
d2xdt2 = (-drag(y[1]-v_wind, rho_l, cw_rocket_side , A_rocket_side)+(F_accel_rocket*np.cos(beta/180*np.pi)))/m_rocket
dxdt = y[1]
d2ydt2 = (-9.81 - drag(y[3], rho_l, cw_rocket_front, A_rocket_front)+(F_accel_rocket*np.sin(beta/180*np.pi)))/m_rocket
dydt = y[3]
else:
d2xdt2 = (-drag(y[1]-v_wind, rho_l, cw_rocket_side , A_rocket_side))/m_rocket
dxdt = y[1]
d2ydt2 = (-9.81 - drag(y[3], rho_l, cw_rocket_front, A_rocket_front))/m_rocket
dydt = y[3]
return dxdt, d2xdt2, dydt, d2ydt2
def wrapperRocket(beta,x_skeet,y_skeet,RocketArgs):
tspan = [0, 100]
maxStep = 0.01
start_vec = [x0_rocket, v0_rocket*np.cos(beta/180*np.pi), y0_rocket, v0_rocket*np.sin(beta/180*np.pi)]
sol2 = solve_ivp(rocket,tspan,start_vec,args=(beta,rho_l,cw_rocket_side, cw_rocket_front,A_rocket_side,A_rocket_front,m_rocket,F_accel_rocket,t_accel_end),max_step=maxStep,dense_output=True)
for i in range(len(x_skeet)):
distance = np.sqrt((sol2.y[0,i]-x_skeet)**2+(sol2.y[2,i]-y_skeet)**2)
return distance
starting_guess = 20 #deg
res = minimize(wrapperRocket, x0 = starting_guess, args = (x_skeet,y_skeet,RocketArgs))
angle = res
print(angle)
Related
import numpy as np
import matplotlib.pyplot as plt
%matplotlib inline
input = [[0,0,1],[0,1,1],[1,0,1],[1,1,1]]
output = [0,1,1,0]
N = np.size(input,0) # number of samples
Ni = np.size(input,1) # dimension of the samples of input
No = 1 # dimension of the sample of output
Nh = 10 # number of hidden units
Ws = 1/4*np.random.rand(Nh,Ni+1)
print(Ws)
Wo = 1/4*np.random.rand(No,Nh)
print(Wo)
alpha = 0.05 # Learning rate
t_ = []
loss_ = []
def ReLU(x):
return np.maximum(0,x)
def sigmoid(x):
return 1/(1+np.exp(-x))
## train the model ====================================================================
for epoch in range(0,3000):
loss = 0
for id_ in range(0,N):
dWs = 0*Ws
dWo = 0*Wo
x = np.append(input[id_],1)
Z_1 = np.dot(Ws,x)
Z_2 = np.dot(Wo,ReLU(Z_1))
y = sigmoid(Z_2)
d = output[id_]
for j in range(0,Nh):
for i in range(0,No):
if Z_1[j] >= 0:
dWo[i,j] = dWo[i,j] + (y[i]-d)*Z_1[j]
#dWo[i,j] = dWo[i,j] + sigmoid(Z_1[j])*(y[i]-d)
else:
dWo[i,j] += 0
Wo = Wo - alpha*dWo
for k in range(0,Ni+1):
for j in range(0,Nh):
for i in range(0,No):
if Z_1[j] >= 0:
dWs[j,k] = dWs[j,k] + x[k]*Wo[i,j]*(y[i]-d)
#dWs[j,k] = dWs[j,k] + x[k]*Wo[i,j]*sigmoid(Z_1[j])*(1-sigmoid(Z_1[j]))*(y[i]-d)
else:
dWs[j,k] += 0
Ws = Ws - alpha*dWs
loss = loss + 1/2*np.linalg.norm(y-d)
if np.mod(epoch,50) == 0:
print(epoch,"-th epoch trained")
t_ = np.append(t_,epoch)
loss_ = np.append(loss_,loss)
fig = plt.figure(num=0,figsize=[10,5])
plt.plot(t_,loss_,marker="")
plt.title('Loss decay')
plt.xlabel('epoch',FontSize=20)
plt.ylabel('Loss',FontSize=20)
plt.show()
## figure out the function shape the model==========================================
xn = np.linspace(0,1,20)
yn = np.linspace(0,1,20)
xm, ym = np.meshgrid(xn, yn)
xx = np.reshape(xm,np.size(xm,0)*np.size(xm,1))
yy = np.reshape(ym,np.size(xm,0)*np.size(xm,1))
Z = []
for id__ in range(0,np.size(xm)):
x = np.append([xx[id__],yy[id__]],[1,1])
Z_1 = np.dot(Ws,x)
y_ = sigmoid(np.dot(Wo,ReLU(Z_1)))
Z = np.append(Z,y_)
fig = plt.figure(num=1,figsize=[10,5])
ax = fig.gca(projection='3d')
surf = ax.plot_surface(xm,ym,np.reshape(Z,(np.size(xm,0),np.size(xm,1))),cmap='coolwarm',linewidth=0,antialiased=False)
print("====================================================================")
plt.show()
## test the trained model ====================================================================
for id_ in range(0,N):
x = np.append(input[id_],1)
Z_1 = np.dot(Ws,x)
y = sigmoid(np.dot(Wo,ReLU(Z_1)))
print(y)
If I try this with sigmoid function, it works fine but when the ReLU activation function is implemented, the the program doesn't learning anything.
The NN consist of 3 input, hidden, output layers and sigmoid activation fuction is implemented for output function. Hand calculation seems fine but can't find the flaw.
The code below with sigmoid activation function works just fine.
import numpy as np
import matplotlib.pyplot as plt
%matplotlib inline
input = [[0,0,1],[0,1,1],[1,0,1],[1,1,1]]
output = [0,1,1,0]
N = np.size(input,0) # number of samples
Ni = np.size(input,1) # dimension of the samples of input
No = 1 # dimension of the sample of output
Nh = 5 # number of hidden units
Ws = 1/4*np.random.rand(Nh,Ni+1)
#print(Ws)
Wo = 1/4*np.random.rand(No,Nh)
#print(Wo)
alpha = 0.1 # Learning rate
t_ = []
loss_ = []
def sigmoid(x):
return 1/(1+np.exp(-x))
## train the model ====================================================================
for epoch in range(0,5000):
loss = 0
for id_ in range(0,N):
dWs = 0*Ws
dWo = 0*Wo
x = np.append(input[id_],1)
Z_1 = np.dot(Ws,x)
A_1 = sigmoid(Z_1)
Z_2 = np.dot(Wo,A_1)
y = sigmoid(Z_2)
d = output[id_]
for j in range(0,Nh):
for i in range(0,No):
dWo[i,j] = dWo[i,j] + sigmoid(Z_1[j])*(y[i]-d)
Wo = Wo - alpha*dWo
for k in range(0,Ni+1):
for j in range(0,Nh):
for i in range(0,No):
dWs[j,k] = dWs[j,k] + x[k]*Wo[i,j]*sigmoid(Z_1[j])*(1-sigmoid(Z_1[j]))*(y[i]-d)
Ws = Ws - alpha*dWs
loss = loss + 1/2*np.linalg.norm(y-d)
if np.mod(epoch,50) == 0:
print(epoch,"-th epoch trained")
t_ = np.append(t_,epoch)
loss_ = np.append(loss_,loss)
fig = plt.figure(num=0,figsize=[10,5])
plt.plot(t_,loss_,marker="")
plt.title('Loss decay')
plt.xlabel('epoch',FontSize=20)
plt.ylabel('Loss',FontSize=20)
plt.show()
## figure out the function shape the model==========================================
xn = np.linspace(0,1,20)
yn = np.linspace(0,1,20)
xm, ym = np.meshgrid(xn, yn)
xx = np.reshape(xm,np.size(xm,0)*np.size(xm,1))
yy = np.reshape(ym,np.size(xm,0)*np.size(xm,1))
Z = []
for id__ in range(0,np.size(xm)):
x = np.append([xx[id__],yy[id__]],[1,1])
Z_1 = np.dot(Ws,x)
y_ = sigmoid(np.dot(Wo,sigmoid(Z_1)))
Z = np.append(Z,y_)
fig = plt.figure(num=1,figsize=[10,5])
ax = fig.gca(projection='3d')
surf = ax.plot_surface(xm,ym,np.reshape(Z,(np.size(xm,0),np.size(xm,1))),cmap='coolwarm',linewidth=0,antialiased=False)
print("====================================================================")
plt.show()
## test the trained model ====================================================================
for id_ in range(0,N):
x = np.append(input[id_],1)
Z_1 = np.dot(Ws,x)
y = sigmoid(np.dot(Wo,sigmoid(Z_1)))
print(y)
I found similar case in Quora.
And have tested it in my networks that involves modelling logics to resolve some noisy cost function.
I found that ReLu outputs are usually blasted all over, by the 3rd layer of MLP, the values before the output have accumulated to thousands if not millions.
And with that, I prefer sigmoid with MLPs. Don't forget, sigmoid limits output to 1, but ReLu does not.
The intuition behind ReLu is that it filters out unneeded info by means of MAX(0,X) function, before forwarded to the next layer of processing. For the same reason you see it being used in Convolution problems. Note: Normalization Layer is used in these cases so that the output values of the nodes will not blast all over.
But in the case of an MLP, you didn't implement any Norm Layer after ReLu, for that reason, it is difficult to model a simple function such as XOR. In short, without Norm Layer, I don't recommend the use of ReLu, although in some cases, it still can function properly.
I work to about missile routing. I calculated throuhout the flight x,y,z coordinates of missile . I have data set about missile x,y,z coordinates. My goal is to move the servo motor according to x, y, z coordinates.
My input is 3-dimensional(x,y,z). I want to simulate in two dimensions. And for this I use vectoral calculation. The servo motor can take values between 0-1. But the result larger than 1 . When the results are reduced at the same rate, result is smaller than 0. But I get still the error
Undefined function 'writePosition' for input arguments of type 'matlab.graphics.chart.primitive.Surface'.
I will be grateful if you could help me.
My data example:
missile_x = 0.015
missile_y = 0.054
missile_z = 0.254
missile_flight = 0.00018794
My flight rotation code:
missile_x = vpa(Xval{id}(k)/10,5)
missile_y = vpa(Yval{id}(k)/10,5)
missile_z = vpa(Zval{id}(k)/10,5)
missile_flight = vpa(0.00555556*(missile_x^2+missile_y^2+missile_z^2)^1/2,5)
writePosition(s, missile_flight);
current_pos = readPosition(s);
current_pos = current_pos*missile_flight;
fprintf('Current motor position is %d degrees\n', current_pos);
pause(2);
Missile X,Y,Z calculation code:
dt = 0.005; %time step
g = 9.81; %gravity
ro = 1.2; %air density
A = pi*(0.2)^2; % reference area
Vmag = 0; % missile velocity vectoral value [m/sn]
t = 0;
T(1) = t;
U(1) = 0; %the missile is initially at rest at t = 0; So the velocity is 0
V(1) = 0;
W(1) = 0;
X(1) = X0;
Y(1) = Y0;
Z(1) = Z0;
n = 1;
h = interp2(x_terrain, y_terrain, h_terrain,X(1), Y(1));
while (Z(n) >= h)
[Thx, Thy, Thz] = thrust(t, Thmag0, theta, phi, Tburn, U(n), V(n), W(n));
Vmag = (U(n)^2 + V(n)^2 + W(n)^2)^(1/2);
Thmag = (Thx^2 + Thy^2 + Thz^2)^(1/2);
m = mass(t, m0, mf, Tburn);
[rho,sound_speed] = atmosphere(Z(n));
Cd = drag_coeff(Vmag,sound_speed);
U(n+1) = U(n) + (Thx/m - (Cd*rho*A/(2*m))*(U(n)*(U(n)^2+V(n)^2+W(n)^2)^(1/2)))*dt;
V(n+1) = V(n) + (Thy/m - (Cd*rho*A/(2*m)*(V(n)*(U(n)^2+V(n)^2+W(n)^2)^(1/2))))*dt;
W(n+1) = W(n) + (Thz/m - (Cd*rho*A/(2*m))*(W(n)*(U(n)^2+V(n)^2+W(n)^2)^(1/2)) - g)*dt;
X(n+1) = X(n) + U(n+1)*dt;
Y(n+1) = Y(n) + V(n+1)*dt;
Z(n+1) = Z(n) + W(n+1)*dt;
h = interp2(x_terrain, y_terrain, h_terrain, ...
X(end), Y(end));
t = t + dt;
T(n+1) = t;
n = n+1 ;
end
Tval = T;
Xval = X;
Yval = Y;
Zval = Z;
My code simulates the shock loading of a rocket upon parachute deployment. When the script is run as a whole the value of the shock goes to infinity. However, when the script is run step by step in the debugger mode it works as intended. I am unsure of what is happening or how to fix it. My code is written as follows:
clear all; close all; clc; tic
h_init = 100; %initial altitude, ft
%INPUTS
E = .38e6; %elastic modulus of nylon, psi
A = (1/16)*(1); %cross sectional area of cord, in^2
CD = 2.20; %parachute coefficient of drag
D_p = 144; %parachute diameter, in
W_p = 2; %weight of parachute, lbm
W_r = 105; %dry weight of rocket, lbm
L = 17*3*12; %length of shock cord, in
v_init = -350; %initial upwards velocity of rocket/parachute, ft/s
dt = 0.00001; %simulation time step, s
t_sim = 4; %total simulation time, s
g = 32.2; %gravitational acceleration, ft/s^2
%CALCULATIONS
k = (3900)./L; %spring constant, lbf/in
S_p = pi*D_p.^2/4/144; %parachute cross sectional area, ft^2
m_p = W_p/g; %mass of parachute, slugs
m_r = W_r./g; %mass of rocket, slugs
N = ceil(t_sim/dt);
N_L = length(W_r);
time = dt:dt:t_sim + dt;
Y1 = zeros(N_L,N);
Y2 = zeros(N_L,N);
V1 = zeros(N_L,N);
V2 = zeros(N_L,N);
FS = zeros(N_L,N);
Dr = zeros(N_L,N);
Shock = zeros(N_L,1);
Exten = zeros(N_L,1);
VDiff = zeros(N_L,1);
DrMax = zeros(N_L,1);
TDrop = zeros(N_L,1);
for j = 1:N_L
v1 = v_init; %initial velocity of parachute after deployment, ft/s
y1 = 0; %initial parachute position relative to deployment altitude, ft
v2 = v_init; %initial velocity of rocket after deployment, ft/s
y2 = 0; %initial position of rocket relative to deployment altitude, ft
Y1(j,1) = y1;
Y2(j,1) = y2;
V1(j,1) = v1;
V2(j,1) = v2;
FS(j,1) = 0;
Dr(j,1) = 0;
slack = 1;
for i = 1:N
rho = findDensity(h_init+y1);%air density, slug/ft^3
Q = 144*0.5*rho*v1^2; %dynamic pressure, psi
D = -sign(v1)*(Q/144)*S_p*CD; %drag, lbf
if y1-y2 >= L/12
delta = 12*(y1-y2)-L; %shock cord extension, in
if slack ==1
TDrop(j) = dt*i;
slack = 0;
end
elseif y2-y1 >= L/12
delta = -12*(y1-y2)-L; %shock cord extension, in
else
delta = 0;
end
fs = k*delta; %spring force, lbf
F1 = D - W_p - fs; %net force on parachute, lbf
F2 = fs - W_r(j); %net force on rocket, lbf
a1 = F1/m_p; %acceleration of parachute, ft/s^2
a2 = F2/m_r(j); %acceleration of rocket, ft/s^2
y1 = y1 + v1*dt; %update parachute position, ft
y2 = y2 + v2*dt; %update rocket position, ft
v1 = v1 + a1*dt; %update parachute velocity, ft/s
v2 = v2 + a2*dt; %update rocket velocity, ft/s
Y1(j,i+1) = y1;
Y2(j,i+1) = y2;
V1(j,i+1) = v1;
V2(j,i+1) = v2;
FS(j,i+1) = fs;
Dr(j,i+1) = D;
end
Shock(j) = max(FS(j,:));
Exten(j) = 12*max(Y1(j,:)-Y2(j,:)-L/12);
VDiff(j) = max(abs(V1(j,:)-V2(j,:)));
DrMax(j) = max(Dr(j,:));
end
H1 = h_init+Y1;
H2 = h_init+Y2;
fprintf('Max Shock Load: %.2f lbf', Shock)
toc
function density = findDensity(h)
temp = findTemp(h);
pressure = findPressure(h);
density = pressure / (1718 * (temp + 459.7));
end
function pressure = findPressure(h)
temp = findTemp(h);
if(h > 82345)
pressure = 51.97 * (((temp + 459.7) / 389.98) ^ -11.388);
elseif(h > 36152)
pressure = 473.1 * exp(1.73 - (0.000048 * h));
else
pressure = 2116 * (((temp + 459.7) / 518.6) ^ 5.256);
end
end
function temp = findTemp(h)
if(h > 82345)
temp = (0.00164 * h) - 205.5;
elseif(h > 36152)
temp = -70;
else
temp = 59 - (0.00356 * h);
end
end
I am wondering what is the fastest convex optimizer in Matlab or is there any way to speed up current solvers? I'm using CVX, but it's taking forever to solve the optimization problem I have.
The optimization I have is to solve
minimize norm(Ax-b, 2)
subject to
x >= 0
and x d <= delta
where the size of A and b are very large.
Is there any way that I can solve this by a least square solver and then transfer it to the constraint version to make it faster?
I'm not sure what x.d <= delta means, but I'll just assume it's supposed to be x <= delta.
You can solve this problem using the projected gradient method or an accelerated projected gradient method (which is just a slight modification of the projected gradient method, which "magically" converges much faster). Here is some python code that shows how to minimize .5|| Ax - b ||^2 subject to the constraint that 0 <= x <= delta using FISTA, which is an accelerated projected gradient method. More details about the projected gradient method and FISTA can be found for example in Boyd's manuscript on proximal algorithms.
import numpy as np
import matplotlib.pyplot as plt
def fista(gradf,proxg,evalf,evalg,x0,params):
# This code does FISTA with line search
maxIter = params['maxIter']
t = params['stepSize'] # Initial step size
showTrigger = params['showTrigger']
increaseFactor = 1.25
decreaseFactor = .5
costs = np.zeros((maxIter,1))
xkm1 = np.copy(x0)
vkm1 = np.copy(x0)
for k in np.arange(1,maxIter+1,dtype = np.double):
costs[k-1] = evalf(xkm1) + evalg(xkm1)
if k % showTrigger == 0:
print "Iteration: " + str(k) + " cost: " + str(costs[k-1])
t = increaseFactor*t
acceptFlag = False
while acceptFlag == False:
if k == 1:
theta = 1
else:
a = tkm1
b = t*(thetakm1**2)
c = -t*(thetakm1**2)
theta = (-b + np.sqrt(b**2 - 4*a*c))/(2*a)
y = (1 - theta)*xkm1 + theta*vkm1
(gradf_y,fy) = gradf(y)
x = proxg(y - t*gradf_y,t)
fx = evalf(x)
if fx <= fy + np.vdot(gradf_y,x - y) + (.5/t)*np.sum((x - y)**2):
acceptFlag = True
else:
t = decreaseFactor*t
tkm1 = t
thetakm1 = theta
vkm1 = xkm1 + (1/theta)*(x - xkm1)
xkm1 = x
return (xkm1,costs)
if __name__ == '__main__':
delta = 5.0
numRows = 300
numCols = 50
A = np.random.randn(numRows,numCols)
ATrans = np.transpose(A)
xTrue = delta*np.random.rand(numCols,1)
b = np.dot(A,xTrue)
noise = .1*np.random.randn(numRows,1)
b = b + noise
def evalf(x):
AxMinusb = np.dot(A, x) - b
val = .5 * np.sum(AxMinusb ** 2)
return val
def gradf(x):
AxMinusb = np.dot(A, x) - b
grad = np.dot(ATrans, AxMinusb)
val = .5 * np.sum(AxMinusb ** 2)
return (grad, val)
def evalg(x):
return 0.0
def proxg(x,t):
return np.maximum(np.minimum(x,delta),0.0)
x0 = np.zeros((numCols,1))
params = {'maxIter': 500, 'stepSize': 1.0, 'showTrigger': 5}
(x,costs) = fista(gradf,proxg,evalf,evalg,x0,params)
plt.figure()
plt.plot(x)
plt.plot(xTrue)
plt.figure()
plt.semilogy(costs)
I have implemented a perspective projection algorithm according to chapter 6 Computer Graphics Principles and Practices (CGP&P) by Foley, van Dam, Feiner, Hughes (2nd edition). I have
N'per = M * Sper * Spar * T (-prp) * R * T (-vrp).
As I understand it, the final image should be in canonical form size of (-1,-1) to (1,1) and z in (0,-1). However, the final image X-Y dimensions (see Figure 1) do not seem correct. I'm mostly trying to understand how the final image size is determined. I have included the matlab code below. My frustum (f) is defined by eyepoint (EP) at a specified lat/lon that has been converted to ECEF; distances: near plane (nDist) = 300; view plane (vDist) = 900; and far plane (fDist) = 25000. A line of sight (LOS) vector created at the EP is the center of projection. The frustum correctly finds and returns the buildings that within it along the LOS. Field of View is (10 deg x 10 deg). Now I'm just trying to project those buildings onto a defined window so that I can "quantize" (paint?) the grid and indicate which building is located at which x,y pair in the view plane. Unfortunately, because the window is not returning at the indicated size, it makes the painting more difficult for me. And besides, I'd just like to know what I'm doing wrong to not end up with the correct dimensions.
Matlab code (no attempts at optimizations or anything. just brute-force implementation!
function iPersProj = getPersProj(bldg, bi, f, plotpersp, fPersPlot)
color = [rand rand rand];
face = eFaces.bottom;
iPersProjBtm = persproj(f, bldg, face);
face = eFaces.top;
iPersProjTop = persproj(f, bldg, face);
iPersProj = [iPersProjTop;iPersProjBtm];
hold on;
scatter3(iPersProjTop(:,1), ...
iPersProjTop(:,2), ...
iPersProjTop(:,3),'+','CData',color);
scatter3(iPersProjBtm(:,1), ...
iPersProjBtm(:,2), ...
iPersProjBtm(:,3),'o','CData',color);
pPersProj=[iPersProjTop;
iPersProjTop(1,:); ...
iPersProjBtm; ...
iPersProjBtm(1,:); ...
iPersProjBtm(2,:); ...
iPersProjTop(4,:); ...
iPersProjTop(3,:); ...
iPersProjBtm(3,:); ...
iPersProjBtm(4,:); ...
iPersProjTop(2,:); ...
iPersProjTop(1,:)];
line (pPersProj(:,1), pPersProj(:,2),'Color',color);
text (pPersProj(1,1), pPersProj(1,2), int2str(bi));
end
function proj = persproj(f, bldg, face)
vrp = f.vC; %center view plane
vpn = f.Z; % LOS for frustum
cop = -f.EP;
F = f.vDist - f.nDist;
B = f.vDist - f.fDist;
umin = -5;
vmin = -5;
umax = 5;
vmax = 5;
R = getrotation (f);
Tvrp = gettranslation(-vrp);
ed = R * Tvrp * [f.EP 1]'; %translate eyepoint to camera?
prp = [0 0 ed(3)];
sh = getsh(prp, umax, umin, vmax, vmin);
Tprp = gettranslation(-prp);
vrpp = -prp(3); %(sh * Tprp * [0;0;0;1]); %vrp-prime per CGP&P
zmin = -(vrpp + F)/(vrpp+B);
zmax = -(vrpp + B)/(vrpp+B);
zprj = -vrpp/(vrpp+B);
sper = getsper(vrpp, B, umax, umin, vmax, vmin);
M=[ 1 0 0 0; ...
0 1 0 0; ...
0 0 1/(1+zmin) -zmin/(1+zmin); ...
0 0 -1 0];
proj = zeros(4,4);
for i=1:4
Q=bldg.coords(i,:,face);
uvdw = M * sper * sh * Tprp * R * Tvrp * [Q';1];
proj (i,1) = uvdw(1);
proj (i,2) = uvdw(2);
proj (i,3) = uvdw(3);
end
end
function sper = getsper (vrpz, B, umax, umin, vmax, vmin)
dx=umax-umin;
dy=vmax-vmin;
sper=zeros(4,4);
sper(1,1) = 2*vrpz/(dx*(vrpz+B));
sper(2,2) = 2*vrpz/(dy*(vrpz+B));
sper(3,3) = -1/(vrpz+B);
sper(4,4) = 1;
end
function sh = getsh (prp, umax, umin, vmax, vmin)
sx=umax+umin;
sy=vmax+vmin;
cw = [sx/2 sy/2 0 1]';
dop = cw - [prp 1]';
shx = - dop(1)/dop(3);
shy = - dop(2)/dop(3);
sh=zeros(4,4);
sh(1,1) = 1;
sh(2,2) = 1;
sh(3,3) = 1;
sh(4,4) = 1;
sh(1,3) = shx;
sh(2,3) = shy;
end
function R = getrotation (f)
rz = f.Z;
rx=cross(f.Y, rz);
rx=rx/norm(rx);
ry=cross(rz,rx);
R=zeros(4,4);
R(1,1:3) = rx;
R(2,1:3) = ry;
R(3,1:3) = rz;
R(4,4) = 1;
end
function T = gettranslation(p)
T = zeros(4,4);
T(1:3,4) = p';
T(1,1) = 1;
T(2,2) = 1;
T(3,3) = 1;
T(4,4) = 1;
end
Figure 1: Prospective Projection but dimensions are not (-1,-1) to (1,1)1