#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Package DLMgeometry contains functions to create grids for the Doublet Lattice 
Method.

This code is part of the SDPMflut Matlab distribution.
Copyright (C) 2025 Grigorios Dimitriadis

This program is free software: you can redistribute it and/or modify it 
under the terms of the GNU General Public License as published by the 
Free Software Foundation, either version 3 of the License, or (at your 
option) any later version.
 
This program is distributed in the hope that it will be useful, but 
WITHOUT ANY WARRANTY; without even the implied warranty of 
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General 
Public License for more details.

You should have received a copy of the GNU General Public License along 
with this program. If not, see <https://www.gnu.org/licenses/>.
"""

import numpy as np
import numpy.matlib
from scipy import linalg
import airfoils

def normalarea(x,y,z):
    # Calculates the unit normal vector and surface area of a near-planar 
    # quadrilateral panel. The vertices of the quadrilateral are stored in 
    # vectors x, y and z. Also calculates a measure of the coplanarity of the 
    # four vertices.
    # x: 4-element array of the x-coordinates of the vertices of the panel
    # y: 4-element array of the y-coordinates of the vertices of the panel
    # z: 4-element array of the z-coordinates of the vertices of the panel
    # normal: 3-lement array of the unit vector normal to the panel
    # s: area of the panel
    # cpln: Measure of the coplanarity of the four vertices.

    # Normal vector from diagonals 13 and 24. The vector is normal to these two
    # diagonals. 
    normal=np.cross(np.array([x[2]-x[0], y[2]-y[0], z[2]-z[0]]),np.array([x[1]-x[3], y[1]-y[3], z[1]-z[3]]))
    normal=normal/linalg.norm(normal)
    # Calculate a second normal from edges 12 and 13. The vector is normal to 
    # these two edges.
    normal2=np.cross(np.array([x[2]-x[0], y[2]-y[0], z[2]-z[0]]),np.array([x[1]-x[0], y[1]-y[0], z[1]-z[0]]))
    mag2=linalg.norm(normal2)
    # Test if the normal and normal2 are colinear. If they are, their cross 
    # product will be equal to zero. Store the magnitude of the cross product 
    # in cpln.
    if mag2 != 0.0:
        normal2=normal2/mag2
        cpln=np.cross(normal,normal2)
        cpln=linalg.norm(cpln)
    else:
        cpln=0.0
    # Enf if
    # Calculate a third normal from diagonal 13 and edge 14. The vector is 
    # normal to these two segments.
    normal3=np.cross(np.array([x[2]-x[0], y[2]-y[0], z[2]-z[0]]),np.array([x[3]-x[0], y[3]-y[0], z[3]-z[0]]))
    mag3=linalg.norm(normal3)
    # Calculate panel area from equation 5.91
    s=mag2/2.0+mag3/2.0;
    
    return normal,s,cpln

def DLMgeometry_trap_fun(body,ibody,m,nhalf,mirroredwing,linchord,linspan,trap,name,rollpitchyaw,rollpitchyaw_cent,lexyz,nmin,bchar):
    # Calculates the coordinates of the panel vertices for the Doublet Lattice
    # Method.
    # body: struct array containing the geometry of all the bodies in the flow.
    #       Each element of body describes a different wing, fairing or 
    #       fuselage
    # ibody: Index of body for which to determine the self-influence
    # m: Number of panels in chordwise direction
    # n: Number of panels in spanwise direction on the half wing.
    # mirroredwing: If mirroredwing=-1: a left half-wing will be created
    #               If mirroredwing=1: a right half-wing will be created
    #               If mirroredwing=2: two mirrored half-wings will be created.
    # linchord: Chordwise panel distribution: linchord=1 constant, 
    #           linchord=2 denser at the leading edge. For the DLM only 
    #           linchord=1 should be used.
    # linspan: Spanwise panel distribution: linspan=1 constant, linspan=2 
    #           denser at the wing tip(s) and midspan. For the DLM only 
    #           linspan=1 should be used.
    # trap: struct array containing wing geometry information arranged in 
    #           trapezoidal sections.
    # name: String of the name of the wing.
    # rollpitchyaw: 3-element array containing rotation angles around the x, y
    #           y and z axes.
    # rollpitchyaw_cent: 3-element of the x, y and z coordinates around which
    #           the rotations in rollpitchyaw are to be applied.
    # lexyz: 3-element of the x, y and z coordinates of the root leading edge
    #           of the wing.
    # nmin: Minimum number of spanwise panels per trapezoidal section.
    # bchar: Characteristic chordwise length used for normalising the geometry.

    # Calculate half span
    bhalf=np.sum(trap['span'])
    # Incremental array of half spans
    bsum=np.zeros(len(trap) + 1)
    bsum[1:len(trap) + 1]=np.cumsum(trap['span'])

    # Calculate the planform area of each trapezoidal section
    Strap=(trap['rootchord']+trap['rootchord']*trap['taper'])*trap['span']/2.0
    # Calculate wing planform area
    S=np.sum(Strap)
    # Calculate wing aspect ratio
    AR=(2.0*bhalf)**2/(2.0*S)

    # Define chordwise panel grid 
    if linchord == 1:
        # Linearly spaced non-dimensional chordwise panel vertex coordinates
        xp=np.linspace(0,1,num=m+1) 
    else:
        # Nonlinearly spaced non-dimensional chordwise panel vertex coordinates from equation 5.182
        xp=1-np.sin(np.linspace(np.pi/2.0,0,num=m+1))
     # End if
    # Two-sided x array for airfoil calculation
    xp2=np.concatenate([np.flip(xp),xp[1:m+1]])
   
    # Distribute spanwise panels over the trapezoidal sections
    n_sec=np.round(trap['span']/bhalf*nhalf)
    n_sec=n_sec.astype(int)
    # Ensure that none of the sections have fewer than nmin spanwise panels
    iko=np.argwhere(n_sec <= nmin)
    if len(iko) > 0:
        n_sec[iko]=3
    # End if
    # Update spanwise panel number
    nhalf=np.sum(n_sec)
 
    if mirroredwing == 2:
        n=2*nhalf # Number of spanwise panels for the full wing
    else:
        n=nhalf
    # end if
    
    # Initialize grid
    Xp0=np.zeros((m+1,nhalf+1))
    Yp0=np.zeros((m+1,nhalf+1))
    Zp0=np.zeros((m+1,nhalf+1))
    Zpcamb=np.zeros((m+1,nhalf+1))
    
    # Cumulative sum of panels
    nsum=np.zeros(len(trap) + 1,dtype=int)
    nsum[1:len(trap) + 1]=np.cumsum(n_sec)
    # Set root x-coordinate of first section to zero
    xrootle=0.0
    # Cycle over the trapezoidal sections
    for i in range(0,len(trap)): 
        # Set up spanwise panel grid
        if linspan == 1:
            # Linearly spaced non-dimensional spanwise panel vertex coordinates
            yp=np.linspace(0,1,num=n_sec[i]+1) 
        else:
            # Nonlinearly spaced non-dimensional spanwise panel vertex coordinates from equation 5.183
            theta=np.linspace(np.pi,0,num=n_sec[i]+1)
            yp=(1+np.cos(theta))/2.0
        # End if        
        # Calculate linear variation of chord length over the trapezoidal section
        cvar=(trap['taper'][i]-1)*trap['rootchord'][i]*np.absolute(yp)+trap['rootchord'][i]
        # Calculate leading edge x-coordinate variation along the section due
        # to sweep
        xsweep=np.tan(trap['sweepLE'][i])*abs(yp)*trap['span'][i]
        # Calculate x-coordinates of panel vertices
        Xp0[:,nsum[i]:nsum[i+1]+1]=np.transpose(np.matlib.repmat(xp,n_sec[i]+1,1))*np.matlib.repmat(cvar,m+1,1)+\
            np.matlib.repmat(xsweep+xrootle,m+1,1)+trap['xledist'][i]
        # Calculate y-coordinates of panel vertices
        Yp0[:,nsum[i]:nsum[i+1]+1]=np.matlib.repmat(yp,m+1,1)*trap['span'][i]+bsum[i]
        
        # Calculate non-dimensional root airfoil shape    
        if trap['rootairfoil'][i] == 'nacafourdigit':
            zp1,zpcamb=airfoils.nacafourdigit(xp2,m,trap['rootairfoilparams'][i][0],trap['rootairfoilparams'][i][1])
        elif trap['rootairfoil'][i] == 'nacafivedigit':
            zp1,zpcamb=airfoils.nacafivedigit(xp2,m,trap['rootairfoilparams'][i][0],trap['rootairfoilparams'][i][1])
        elif trap['rootairfoil'][i] == 'LANNairfoil':
            zp1,zpcamb=airfoils.nacafivedigit(xp2,m,trap['rootairfoilparams'][i][0],trap['rootairfoilparams'][i][1])
        elif trap['rootairfoil'][i] == 'NACA65A004':
            zp1,zpcamb=airfoils.NACA65A004(xp2,m,trap['rootairfoilparams'][i][0])    
        elif trap['rootairfoil'][i] == 'NASASC20414':
            zp1,zpcamb=airfoils.NASASC20414(xp2,m,trap['rootairfoilparams'][i][0])    
        elif trap['rootairfoil'][i] == 'NACA64A010':
            zp1,zpcamb=airfoils.NACA64A010(xp2,m,trap['rootairfoilparams'][i][0])   
        elif trap['rootairfoil'][i] == 'NACA64A010_45':
            zp1,zpcamb=airfoils.NACA64A010_45(xp2,m,trap['rootairfoilparams'][i][0])   
        elif trap['rootairfoil'][i] == 'flatplate':
            zp1,zpcamb=airfoils.flatplate(xp2,m)  
        elif trap['rootairfoil'][i] == 'flatplate_wedge':
            zp1,zpcamb=airfoils.flatplate_wedge(xp2,m,trap['rootairfoilparams'][i][0],trap['rootairfoilparams'][i][1])  
        elif trap['rootairfoil'][i] == 'biconvex':
            zp1,zpcamb=airfoils.biconvex(xp2,m,trap['rootairfoilparams'][i][0])  
        # End if    
        # For the DLM we only need the camber line
        zp1=zpcamb
        
        # Calculate non-dimensional tip airfoil shape    
        if trap['tipairfoil'][i] == 'nacafourdigit':
            zp2,zpcamb=airfoils.nacafourdigit(xp2,m,trap['tipairfoilparams'][i][0],trap['tipairfoilparams'][i][1])
        elif trap['tipairfoil'][i] == 'nacafivedigit':
            zp2,zpcamb=airfoils.nacafivedigit(xp2,m,trap['tipairfoilparams'][i][0],trap['tipairfoilparams'][i][1])
        elif trap['tipairfoil'][i] == 'LANNairfoil':
            zp2,zpcamb=airfoils.nacafivedigit(xp2,m,trap['tipairfoilparams'][i][0],trap['tipairfoilparams'][i][1])
        elif trap['tipairfoil'][i] == 'NACA65A004':
            zp2,zpcamb=airfoils.NACA65A004(xp2,m,trap['tipairfoilparams'][i][0])    
        elif trap['tipairfoil'][i] == 'NASASC20414':
            zp2,zpcamb=airfoils.NASASC20414(xp2,m,trap['tipairfoilparams'][i][0])  
        elif trap['tipairfoil'][i] == 'NACA64A010':
            zp2,zpcamb=airfoils.NACA64A010(xp2,m,trap['tipairfoilparams'][i][0])  
        elif trap['tipairfoil'][i] == 'NACA64A010_45':
            zp2,zpcamb=airfoils.NACA64A010_45(xp2,m,trap['tipairfoilparams'][i][0])  
        elif trap['tipairfoil'][i] == 'flatplate':
            zp2,zpcamb=airfoils.flatplate(xp2,m)  
        elif trap['rootairfoil'][i] == 'flatplate_wedge':
            zp2,zpcamb=airfoils.flatplate_wedge(xp2,m,trap['rootairfoilparams'][i][0],trap['rootairfoilparams'][i][1])  
        elif trap['tipairfoil'][i] == 'biconvex':
            zp2,zpcamb=airfoils.biconvex(xp2,m,trap['tipairfoilparams'][i][0])  
        # End if        
        # For the DLM we only need the camber line
        zp2=zpcamb
        
        # Interpolate linearly between the root and tip airfoils  
        zpmat=np.zeros((m+1,n_sec[i]+1))
        for iy in range(0,n_sec[i]+1): 
            zpmat[:,iy]=(zp2-zp1)*yp[iy]+zp1
        # End for
        
        # Assign camber coordinates of panel vertices
        if trap['rootairfoil'][i] == 'flatplate':
            Zpcamb[:,nsum[i]:nsum[i+1]+1]=np.zeros((m+1,n_sec[i]+1))
        else:
            Zpcamb[:,nsum[i]:nsum[i+1]+1]=zpmat*np.matlib.repmat(cvar,m+1,1)
        # End for
        # Calculate  root x-coordinate of next section
        xrootle=xrootle+xsweep[n_sec[i]]+trap['xledist'][i]
    # End for
        
    # Calculate derivatives of camber lines in x direction
    Zpcamb_diff=np.zeros((m+1,nhalf+1))
    Zpcamb_diff[0,:]=(Zpcamb[1,:]-Zpcamb[0,:])/(xp[1]-xp[0])
    for i in range (1,m):
        Zpcamb_diff[i,:]=(Zpcamb[i+1,:]-Zpcamb[i-1,:])/(xp[i+1]-xp[i-1])
    # End for
    Zpcamb_diff[m,:]=(Zpcamb[m,:]-Zpcamb[m-1,:])/(xp[m]-xp[m-1])
    
    # Calculate non-dimensional spanwise coordinates of all wing panel vertices
    yp=Yp0[0,:]/bhalf

    # Dihedral calculation
    # Calculate the difference in dihedral angle between sections
    if len(trap) == 1:
        dihedral_diff=np.array(trap['dihedral'][0],ndmin=1)
    else:
        dihedral_diff=np.concatenate([np.array([trap['dihedral'][0]]), np.diff(trap['dihedral'])])  
    # End if
    # Apply dihedral angle
    for i in range(0,len(trap)): 
        # Centre of rotation (root leading edge of trapezoidal section)
        xyzcent=np.array([Xp0[1,nsum[i]], Yp0[1,nsum[i]], Zp0[1,nsum[i]]])
        # Rotation matrix
        Rx=np.array([[1, 0, 0],[0, np.cos(dihedral_diff[i]), -np.sin(dihedral_diff[i])], \
                     [0, np.sin(dihedral_diff[i]), np.cos(dihedral_diff[i])]])
        # Apply dihedral rotation around root leading edge of trapezoidal section
        xx=np.reshape(Xp0[:,nsum[i]+1:nhalf+1],(1,(m+1)*(nhalf-nsum[i])),order='C')-xyzcent[0]
        yy=np.reshape(Yp0[:,nsum[i]+1:nhalf+1],(1,(m+1)*(nhalf-nsum[i])),order='C')-xyzcent[1]
        zz=np.reshape(Zp0[:,nsum[i]+1:nhalf+1],(1,(m+1)*(nhalf-nsum[i])),order='C')-xyzcent[2]
        dummy=Rx @ np.concatenate((xx,yy,zz),axis=0)
        # Assign rotated coordinates
        Xp0[:,nsum[i]+1:nhalf+1]=np.reshape(dummy[0,:],(m+1,nhalf-nsum[i]),order='C')+xyzcent[0]
        Yp0[:,nsum[i]+1:nhalf+1]=np.reshape(dummy[1,:],(m+1,nhalf-nsum[i]),order='C')+xyzcent[1]
        Zp0[:,nsum[i]+1:nhalf+1]=np.reshape(dummy[2,:],(m+1,nhalf-nsum[i]),order='C')+xyzcent[2]
        if i > 0:
            # Also rotate the root of the current trapezoidal section (tip of
            # previous trapezoidal section) by half the dihedral angle
            # difference.
            # Rotation matrix
            Rx=np.array([[1, 0, 0],[0, np.cos(dihedral_diff[i]/2.0), -np.sin(dihedral_diff[i]/2.0)], \
                        [0, np.sin(dihedral_diff[i]/2.0), np.cos(dihedral_diff[i]/2.0)]])
            # Apply dihedral rotation around root leading edge of trapezoidal section
            xyz=np.zeros((3,m+1))
            xyz[0,:]=Xp0[:,nsum[i]]-xyzcent[0]
            xyz[1,:]=Yp0[:,nsum[i]]-xyzcent[1]
            xyz[2,:]=Zp0[:,nsum[i]]-xyzcent[2]
            dummy=Rx @ xyz
            # Assign rotated coordinates
            Xp0[:,nsum[i]]=dummy[0,:]+xyzcent[0]
            Yp0[:,nsum[i]]=dummy[1,:]+xyzcent[1]
            Zp0[:,nsum[i]]=dummy[2,:]+xyzcent[2]
    # End for
    
    # Re-arrange wing panels if necessary 
    if mirroredwing == -1:
        # Left half-wing has been requested, mirror panel vertices
        Xp0=np.flip(Xp0,axis=1)
        Yp0=-np.flip(Yp0,axis=1)
        yp=-np.flip(yp)
        Zp0=np.flip(Zp0,axis=1)
        Zpcamb_diff=np.flip(Zpcamb_diff,axis=1)
    elif mirroredwing == 2:
        # Full wing has been requested, mirror panel vertices and concatenate with original
        Xp0=np.concatenate([np.flip(Xp0,axis=1),Xp0[:,1:nhalf+1]],axis=1)
        Yp0=np.concatenate([-np.flip(Yp0,axis=1),Yp0[:,1:nhalf+1]],axis=1)
        yp=np.concatenate([-np.flip(yp),yp[1:nhalf+1]],axis=0)
        Zp0=np.concatenate([np.flip(Zp0,axis=1),Zp0[:,1:nhalf+1]],axis=1)
        Zpcamb_diff=np.concatenate([np.flip(Zpcamb_diff,axis=1),Zpcamb_diff[:,1:nhalf+1]],axis=1)
    # End if
    
    # Move entire wing to required leading edge position
    Xp0=Xp0+lexyz[0]
    Yp0=Yp0+lexyz[1]
    Zp0=Zp0+lexyz[2]

    # Roll the entire wing
    if rollpitchyaw[0] != 0.0 :
        # Rotation angle
        rollangle=rollpitchyaw[0]
        # Rotation centre
        xf=rollpitchyaw_cent[0]
        yf=rollpitchyaw_cent[1]
        zf=rollpitchyaw_cent[2]
        # Rotation matrix
        Rx=np.array([[1.0, 0.0, 0.0], \
                     [0.0, np.cos(rollangle), -np.sin(rollangle)], \
                     [0.0, np.sin(rollangle), np.cos(rollangle)]])
        # Apply roll rotation around rotation centre
        xx=np.reshape(Xp0,(1,(m+1)*(n+1)),order='C')-xf
        yy=np.reshape(Yp0,(1,(m+1)*(n+1)),order='C')-yf
        zz=np.reshape(Zp0,(1,(m+1)*(n+1)),order='C')-zf
        dummy=Rx @ np.concatenate((xx,yy,zz),axis=0)
        # Assign rotated coordinates
        Xp0=np.reshape(dummy[0,:],(m+1,n+1),order='C')+xf
        Yp0=np.reshape(dummy[1,:],(m+1,n+1),order='C')+yf
        Zp0=np.reshape(dummy[2,:],(m+1,n+1),order='C')+zf
    # End if
    
    # Pitch the entire wing
    if rollpitchyaw[1] != 0.0 :
        # Rotation angle
        pitchangle=rollpitchyaw[1]
        # Rotation centre
        xf=rollpitchyaw_cent[0]
        yf=rollpitchyaw_cent[1]
        zf=rollpitchyaw_cent[2]
        # Rotation matrix
        Ry=np.array([[np.cos(pitchangle), 0.0, np.sin(pitchangle)], \
                     [0.0, 1.0, 0.0], \
                     [-np.sin(pitchangle), 0.0, np.cos(pitchangle)]])
        # Apply pitch rotation around rotation centre
        xx=np.reshape(Xp0,(1,(m+1)*(n+1)),order='C')-xf
        yy=np.reshape(Yp0,(1,(m+1)*(n+1)),order='C')-yf
        zz=np.reshape(Zp0,(1,(m+1)*(n+1)),order='C')-zf
        dummy=Ry @ np.concatenate((xx,yy,zz),axis=0)
        # Assign rotated coordinates
        Xp0=np.reshape(dummy[0,:],(m+1,n+1),order='C')+xf
        Yp0=np.reshape(dummy[1,:],(m+1,n+1),order='C')+yf
        Zp0=np.reshape(dummy[2,:],(m+1,n+1),order='C')+zf
    # End if

    # Yaw the entire wing
    if rollpitchyaw[2] != 0.0 :
        # Rotation angle
        yawangle=rollpitchyaw[2]
        # Rotation centre
        xf=rollpitchyaw_cent[0]
        yf=rollpitchyaw_cent[1]
        zf=rollpitchyaw_cent[2]
        # Rotation matrix
        Rz=np.array([[np.cos(yawangle), -np.sin(yawangle),0.0], \
                     [np.sin(yawangle), np.cos(yawangle),0.0], \
                     [0.0, 0.0, 1.0]])
        # Apply yaw rotation around rotation centre
        xx=np.reshape(Xp0,(1,(m+1)*(n+1)),order='C')-xf
        yy=np.reshape(Yp0,(1,(m+1)*(n+1)),order='C')-yf
        zz=np.reshape(Zp0,(1,(m+1)*(n+1)),order='C')-zf
        dummy=Rz @ np.concatenate((xx,yy,zz),axis=0)
        # Assign rotated coordinates
        Xp0=np.reshape(dummy[0,:],(m+1,n+1),order='C')+xf
        Yp0=np.reshape(dummy[1,:],(m+1,n+1),order='C')+yf
        Zp0=np.reshape(dummy[2,:],(m+1,n+1),order='C')+zf
    # End if
    
    # Non-dimensional spanwise control points coordinates
    yc=(yp[1:n+1]+yp[0:n])/2.0
    
    # Calculate scaled panel vecrtices, control points, chordwise and spanwise lengths of panels,
    # endpoints and midpoints of doublet linear, and areas
    Xp,Yp,Zp,Schar,s,Deltax,Deltay,Xd,Yd,Zd,Xm,Ym,Zm,Xc,Yc,Zc,Zccamb_diff,nx,ny,nz,gammas=DLM_control_areas_lengths(Xp0,Yp0,Zp0,Zpcamb_diff,S,bchar)
   
    # Reshape control point coordinate matrices into vectors
    Xcall=np.reshape(Xc,(m*n,1),order='C')
    Ycall=np.reshape(Yc,(m*n,1),order='C')
    Zcall=np.reshape(Zc,(m*n,1),order='C')
    # Reshape unit normal component matrices into vectors
    nxall=np.reshape(nx,(m*n,1),order='C')
    nyall=np.reshape(ny,(m*n,1),order='C')
    nzall=np.reshape(nz,(m*n,1),order='C')    
    # Reshape panel area matrix into a vector
    sall=np.reshape(s,(m*n,1),order='C')
    # Reshape camber derivative matrix into a vector
    Zccamb_diffall=np.reshape(Zccamb_diff,(m*n,1),order='C')
    
    # Assign data to body struct array
    body['bchar'][ibody]=bchar
    body['Xp0'][ibody]=Xp0
    body['Yp0'][ibody]=Yp0
    body['Zp0'][ibody]=Zp0
    body['Xc'][ibody]=Xc
    body['Yc'][ibody]=Yc
    body['Zc'][ibody]=Zc
    body['Xcall'][ibody]=Xcall
    body['Ycall'][ibody]=Ycall
    body['Zcall'][ibody]=Zcall
    body['Xc0'][ibody]=Xc*bchar
    body['Yc0'][ibody]=Yc*bchar
    body['Zc0'][ibody]=Zc*bchar
    body['Xc0all'][ibody]=Xcall*bchar
    body['Yc0all'][ibody]=Ycall*bchar
    body['Zc0all'][ibody]=Zcall*bchar
    body['nx'][ibody]=nx
    body['ny'][ibody]=ny
    body['nz'][ibody]=nz
    body['nxall'][ibody]=nxall
    body['nyall'][ibody]=nyall
    body['nzall'][ibody]=nzall
    body['Xp'][ibody]=Xp
    body['Yp'][ibody]=Yp
    body['Zp'][ibody]=Zp
    body['Xm'][ibody]=Xm
    body['Ym'][ibody]=Ym
    body['Zm'][ibody]=Zm
    body['Xd'][ibody]=Xd
    body['Yd'][ibody]=Yd
    body['Zd'][ibody]=Zd
    body['Deltax'][ibody]=Deltax
    body['Deltay'][ibody]=Deltay
    body['s'][ibody]=s
    body['sall'][ibody]=sall
    body['s0'][ibody]=s*bchar**2
    body['s0all'][ibody]=sall*bchar**2
    body['gammas'][ibody]=gammas
    body['m'][ibody]=m
    body['n'][ibody]=n
    body['c0'][ibody]=trap['rootchord'][0]
    body['b'][ibody]=2*bhalf
    body['yc'][ibody]=yc
    body['AR'][ibody]=AR
    body['S'][ibody]=S
    body['Schar'][ibody]=Schar
    body['name'][ibody]=name
    body['Zccamb_diff'][ibody]=Zccamb_diff
    body['Zccamb_diffall'][ibody]=Zccamb_diffall

    return body

def DLM_control_areas_lengths(Xp0,Yp0,Zp0,Zpcamb_diff,S,bchar):   
    # Calculates the control points, normal unit vectors, doublet line vertices,
    # surface areas, flatness, mean chords, mean spans, dihedral angles,
    # midpoints of doublet lines and camber derivatives of the panels
    # whose endpoints are given in matrices Xp0, Yp0 and Zp0. 
    # Xp0: m*n array of x-coordinates of panels
    # Yp0: m*n array of y-coordinates of panels
    # Zp0: m*n array of z-coordinates of panels
    # Zpcamb_diff: m*n array of camber derivative at the vertices of the panels.
    # S: Planform area of wing
    # bchar: Characteristic chordwise length used for normalising the geometry.
    # Xp: m*n array of normalized x-coordinates of panels
    # Yp: m*n array of normalized y-coordinates of panels
    # Zp: m*n array of normalized z-coordinates of panels
    # Deltax: m*n array of normalized mean chords of panels
    # Deltay: m*n array of normalized mean spans of panels
    # s: m*n array of normalized areas of panels
    # Xd: m*(n+1) array of normalized x-coordinates of doublet lines
    # Yd: m*(n+1) array of normalized y-coordinates of doublet lines
    # Zd: m*(n+1) array of normalized z-coordinates of doublet lines
    # Xm: m*n array of normalized x-coordinates of doublet line midpoints
    # Ym: m*n array of normalized y-coordinates of doublet line midpoints
    # Zm: m*n array of normalized z-coordinates of doublet line midpoints
    # Xc: m*n array of normalized x-coordinates of control points
    # Yc: m*n array of normalized y-coordinates of control points
    # Zc: m*n array of normalized z-coordinates of control points
    # Zccamb_diff: m*n array of camber derivative at the control points
    # nx: m*n array of x-components of unit normal vectors
    # ny: m*n array of y-components of unit normal vectors
    # nz: m*n array of z-components of unit normal vectors
    # gammas: m*n array of dihedral angles of panels, as defined for the 
    #         nonplanar DLM.

    # Calculate m+1 and n+1
    mp1,np1=Xp0.shape
    # Calculate m and n
    m=mp1-1
    n=np1-1
    # Non-dimensionalize wing panel vertices as in equation 6.48
    Xp=Xp0/bchar        # x-coordinates
    Yp=Yp0/bchar        # y-coordinates
    Zp=Zp0/bchar        # z-coordinates
    # Non-dimensionalize wing area
    Schar=S/bchar**2
    # Initialize scaled panel area array
    s=np.zeros((m,n))
    # Initialize unit normal vector component arrays
    nx=np.zeros((m, n))
    ny=np.zeros((m, n))
    nz=np.zeros((m, n))
    # Initialize angle of normal to vertical array
    gammas=np.zeros((m, n))
    # Cycle through the panels
    for i in range(0,m): 
        for j in range(0,n): 
            x=np.array([Xp[i,j], Xp[i,j+1], Xp[i+1,j+1], Xp[i+1,j]])
            y=np.array([Yp[i,j], Yp[i,j+1], Yp[i+1,j+1], Yp[i+1,j]])
            z=np.array([Zp[i,j], Zp[i,j+1], Zp[i+1,j+1], Zp[i+1,j]])
            # Calculate the normal vectors and panel areas
            N,ss,cplnn=normalarea(x,y,z)
            nx[i,j]  = N[0]
            ny[i,j]  = N[1]
            nz[i,j]  = N[2]
            s[i,j] = ss
            # Calculate angle of normal to vertical
            gammas[i,j]=-np.sign(N[1])*np.arccos(np.dot(N,np.array([0,0,1])))
        # End for
    # End for
    
    # Calculate mean chord of each panel as in figure 6.3(b)
    Deltax=(Xp[1:m+1,0:n]-Xp[0:m,0:n]+Xp[1:m+1,1:n+1]-Xp[0:m,1:n+1])/2.0

    # Calculate half-span of each panel as in figure 6.3(b)
    Deltay = np.sqrt((Yp[0:m,1:n+1]-Yp[0:m,0:n])**2.0+(Zp[0:m,1:n+1]-Zp[0:m,0:n])**2.0)/2.0
       
    # Calculate the two vertices of the doublet lines on each panel  as in 
    # figure 6.3(b)
    Xd=(3.0*Xp[0:m,:]+Xp[1:m+1,:])/4.0
    Yd=Yp[0:m,:]
    Zd=(3.0*Zp[0:m,:]+Zp[1:m+1,:])/4.0

    # Calculate the midpoints of the doublet lines on each panel  as in figure 
    # 6.3(b)
    Xm=(Xd[:,1:n+1]+Xd[:,0:n])/2.0
    Ym=(Yd[:,1:n+1]+Yd[:,0:n])/2.0
    Zm=(Zd[:,1:n+1]+Zd[:,0:n])/2.0
            
    # Calculate panel control points  as in figure 6.3(b)
    dummy=(Xp[0:m,:]+3.0*Xp[1:m+1,:])/4.0
    Xc=(dummy[:,1:n+1]+dummy[:,0:n])/2.0
    Yc=(Yp[0:m,0:n]+Yp[0:m,1:n+1])/2.0
    dummy=(Zp[0:m,:]+3.0*Zp[1:m+1,:])/4.0
    Zc=(dummy[:,1:n+1]+dummy[:,0:n])/2.0

    # Calculate camber derivative on control points  as in figure 6.3(b)
    dummy=(Zpcamb_diff[0:m,:]+3.0*Zpcamb_diff[1:m+1,:])/4.0
    Zccamb_diff=(dummy[:,1:n+1]+dummy[:,0:n])/2.0

    return Xp,Yp,Zp,Schar,s,Deltax,Deltay,Xd,Yd,Zd,Xm,Ym,Zm,Xc,Yc,Zc,Zccamb_diff,nx,ny,nz,gammas

