-- ****************************************************************
-- * *
-- * Matrix_Package * SPEC
-- * *
-- ****************************************************************
generic
type ELEMENT is digits <>;
type INDEX is range <>;
type VECTOR is array (INDEX range <>) of ELEMENT ;
type MATRIX is array (INDEX range <>, INDEX range <>) of ELEMENT;
package MATRIX_PACKAGE is
--| Purpose
--| This package is a general purpose matrix package. It defines data
--| types VECTOR and MATRIX, and contains functions to perform general
--| matrix algebra operations.
--|
--| Initialization Exceptions (none)
--|
--| Notes
--| Not all MIL-HDBK-1804 PDL annotations are used in this package
--| due to its simplicity.
--|
--| Modifications
--| Author: Dr. Roger Lee, Naval Air Development Center
--| Art Adamson, Consultant
-- | Louis Granger, Generic part
-- Types
subtype VEC2T is VECTOR (INDEX range 1 .. 2) ;
subtype VEC3T is VECTOR (INDEX range 1 .. 3) ;
type MATR2T is array (INDEX range <>) of VEC2T;
-- Exceptions
INCOMPARABLE_DIMENSION : exception; -- the dimension of matrices
-- or vectors to be operated are
-- incomparable
SINGULAR : exception; -- matrix to be inverted is singular
-- Operations
function TRANSPOSE (A : MATRIX) return MATRIX ; -- transpose of matrix
function TRANSPOSE (A : VECTOR) return VECTOR ; -- transpose of vector
function "+" (A : VECTOR; B : VECTOR) return VECTOR ; -- sum of vector
function "+" (A : MATRIX; B : MATRIX) return MATRIX ; -- sum of matrix
function "+" (A : ELEMENT; B : VECTOR) return VECTOR ;
-- float added to, each term of matrix
function "+" (A : VEC2T; B : MATR2T) return MATR2T ;
-- Vec2T added to, each term of MATR2T
function "+" (A : MATR2T; B : MATR2T) return MATR2T ;
-- Corressponding terms added.
function "-" (A : VECTOR; B : VECTOR) return VECTOR ;
-- difference of vector
function "-" (A : MATRIX; B : MATRIX) return MATRIX ;
-- difference of matrix
function "*" (A : ELEMENT; B : VECTOR) return VECTOR ;
-- scalar, vector multiplication
function "*" (A : VECTOR; B : ELEMENT) return VECTOR ;
-- vector, scalar multiplication
function "*" (A : VECTOR; B : VECTOR) return ELEMENT ;
-- inner(dot) product of two vectors
function "*" (A : MATRIX; B : VECTOR) return VECTOR ;
-- matrix,column vector multiplication
function MAT4MULT (UL : MATRIX; UR : MATRIX; BL : MATRIX; BR : MATRIX;
B : VECTOR) return VECTOR ;
-- large matrix broken into 4 smaller ones, column vector multiplication
-- (upper left, upper right, bottom left, bottom right--all square)
function "*" (A : VECTOR; B : MATRIX) return VECTOR ;
-- row vector,matrix multiplication
function "*" (A : ELEMENT; B : MATRIX) return MATRIX ;
-- scalar, matrix multiplication
function "*" (A : MATRIX; B : ELEMENT) return MATRIX ;
-- matrix, scalar multiplication
function "*" (A : MATRIX; B : MATRIX) return MATRIX ;
-- matrix, matrix multiplication
function "*" (A : ELEMENT; B : MATR2T) return MATR2T ;
-- Multiplies each term of a MATR2T by a float
function "*" (A : VEC2T; B : MATR2T) return VECTOR ;
-- Dot product of each term of MATR2T by a VEC2T, return array of floats
function "*" (A : VECTOR; B : MATR2T) return MATR2T ;
--Multiplies each term of VEC2T by a corresponding float from a VECTOR
function "**" (A : MATRIX; P : INTEGER) return MATRIX;
-- square matrix raised to integer power
-- if P = -1, we invert the matrix
function "**" (A : VECTOR; B : VECTOR) return VECTOR ;
-- A X B = ab sin(theta) a direction
--perpendicular to plane of A & B.
function JCROSS (A : VEC2T) return VEC2T ;
--Rotates Vec2T 90 degrees CW.
function JCROSS (A : MATR2T) return MATR2T ;
--Rotates Vec2T's 90 degrees CW.
function ROTX (A : VEC2T) return VEC2T ;
--Rotates Vec2T 180 degrees about the X axis.
function ROTY (A : VEC2T) return VEC2T ;
--Rotates Vec2T 180 degrees about the Y axis.
function AXBDOTJ (A : VEC2T; B : VEC2T) return ELEMENT;
--Gets magnitude of A cross B for 2 2D vectors.
function GETTAN (A : VEC2T; B : VEC2T) return ELEMENT;
--Gets TAN(THETA) between 2 2D vectors.
end MATRIX_PACKAGE;
-- ****************************************************************
-- * *
-- * Matrix_Package * BODY
-- * *
-- ****************************************************************
with TEXT_IO;
use TEXT_IO;
package body MATRIX_PACKAGE is
--| Notes
--| Modifications by Art Adamson --
-- Mod by A. P. Adamson..July 1990..Added cross product of vectors.
-- The added function Vector1 ** Vector2 return Vector3 is limited to
-- 3-D vectors. Vector3 = Vector1 cross Vector2.
-- Mod by A. P. Adamson..Oct. 6, 1990..Added VEC2T, a 2D VECTOR subtype.
-- Mod by A. P. Adamson..Oct. 6, 1990..Added VEC3T, a 3D VECTOR subtype.
-- Mod by A. P. Adamson..Oct. 6, 1990..Added MATR2T, a VECTOR but with
-- elements of VEC2T rather than float.
-- Mod by A. P. Adamson..Oct. 6, 1990..Added +, an operation to add a float
-- to each term of a VECTOR.
-- Mod by A. P. Adamson..Oct. 6, 1990..Added *, an operation to multiply
-- each VEC2T of a MATR2T by a single float.
-- Mod by A. P. Adamson..Oct. 6, 1990..Added *, an operation to dot product
-- each VEC2T of a MATR2T by a single VEC2T.
-- Mod by A. P. Adamson..Oct. 15, 1990..Added JCROSS, an operation to cross
-- product a fictitious unit vector j in y direction with a VEC2T having
-- components only in the x and z directions. Result is a VEC2T. Used to
-- allow direct translation of certain 3d vector formulas into 2d space.
-- Net effect is to rotate the vec2t 90 degrees CW.
-- Mod by A. P. Adamson..Oct. 15, 1990..Added + , an operation to add a
-- VEC2T to each term of a MATR2T.
-- Mod by A. P. Adamson..Oct. 15, 1990..Added * , an operation to multiply
-- each term of a MATR2T by a correspunding float term from a vector.
-- Mod by A. P. Adamson..Oct. 15, 1990..Added + , an operation to add
-- corresponding VEC2T's of two MATR2T and return MATR2T.
-- Mod by A. P. Adamson..Nov. 3, 1990..Added ROTX and ROTY operations to
-- rotate a VEC2T 180 degrees about the X axis or the Y axis.
-- Mod by A. P. Adamson..Nov. 11, 1990..Added MAT4MULT operation to
-- column multiply 4 square input matricees (the 4 corner quarters of a lar
-- ger
-- matrix) by a column vector and return a column vector. Useful when a mat
-- rix
-- is too large for the memory capacity.
-- Mod by A. P. Adamson..Jan. 1, 1991..Added aXbDOTj operation to
-- give scalar = mag of A cross B for 2 VEC2T vectors.
-- VEC2T has no third dimensoin so the cross product is not possible.
-- Mod by A. P. Adamson..Jan. 1, 1991..Added GETTAN operation to get the T
-- AN
-- of angle theta between 2 VEC2T vectors.
-- Mod by A. P. Adamson..Feb. 15, 1992..Added GETTAN operation protection
--for divide by 0 when angle = PI/2.
---
function TRANSPOSE (A : MATRIX) return MATRIX is
B : MATRIX (A'FIRST (2) .. A'LAST (2), A'FIRST (1) .. A'LAST (1)) ;
-- ****************************************************************
-- This function performs the tranpose of input matrix A
-- ******************************************************************
begin
for I in A'RANGE (2) loop
for J in A'RANGE (1) loop
B (I, J) := A (J, I) ;
end loop ;
end loop ;
return B ;
end TRANSPOSE;
---
function TRANSPOSE (A : VECTOR) return VECTOR is
-- *****************************************************************
-- This function returns the transpose of a vector. In programming
-- a vector is always stored as one-dimensional array. Therefore,
-- there is no difference between row vector and column vector.
-- Thus, this function just returns the input vector (do nothing ).
-- *****************************************************************
begin
return A;
end TRANSPOSE;
---
function "+" (A : VECTOR; B : VECTOR) return VECTOR is
C : VECTOR (A'FIRST .. A'LAST) ;
-- **************************************************************
-- This function performs the addition of vector A and vector B
-- resulting in a vector. Comparability of dimensions is checked.
-- **************************************************************
begin
if A'FIRST /= B'FIRST or A'LAST /= B'LAST then
raise INCOMPARABLE_DIMENSION;
end if ;
for I in A'RANGE loop
C (I) := A (I) + B (I) ;
end loop ;
return C ;
end "+";
---
function "+" (A : ELEMENT; B : VECTOR) return VECTOR is
C : VECTOR (B'FIRST .. B'LAST) ;
-- **************************************************************
-- This function performs the addition of a FLOAT A to each term
-- of vector B resulting in a vector.
-- **************************************************************
begin
for I in B'RANGE loop
C (I) := A + B (I) ;
end loop ;
return C ;
end "+";
---
function "+" (A : MATRIX; B : MATRIX) return MATRIX is
C : MATRIX (A'FIRST (1) .. A'LAST (1), A'FIRST (2) .. A'LAST (2)) ;
-- *******************************************************************
-- This function performs the addition of matrix A and matrix B
-- resulting in a matrix. Comparability of dimensions is checked.
-- *******************************************************************
begin
if (A'FIRST (1) /= B'FIRST (1) or A'LAST (1) /= B'LAST (1))
or (A'FIRST (2) /= B'FIRST (2) or A'LAST (2) /= B'LAST (2)) then
raise INCOMPARABLE_DIMENSION;
end if ;
for I in A'RANGE (1) loop
for J in A'RANGE (2) loop
C (I, J) := A (I, J) + B (I, J) ;
end loop ;
end loop ;
return C ;
end "+";
--
function "+" (A : VEC2T; B : MATR2T) return MATR2T is
TEMP : MATR2T (B'RANGE);
-- **********************************************************************
-- Vec2T added to, each term of MATR2T
-- **********************************************************************
begin
for I in B'RANGE loop
TEMP (I) := A + B (I);
end loop;
return TEMP;
end "+";
--
function "+" (A : MATR2T; B : MATR2T) return MATR2T is
TEMP : MATR2T (B'RANGE);
--***************************************************************
-- Vec2T added to, each term of MATR2T
--***************************************************************
begin
for I in B'RANGE loop
TEMP (I) := A (I) + B (I);
end loop;
return TEMP;
end "+";
--
function "-" (A : VECTOR; B : VECTOR) return VECTOR is
C : VECTOR (A'FIRST .. A'LAST) ;
-- ******************************************************************
-- This function performs the subtraction of vector B from vector A
-- resulting in a vector. Comparability of dimensions is checked.
-- ******************************************************************
begin
if A'FIRST /= B'FIRST or A'LAST /= B'LAST then
raise INCOMPARABLE_DIMENSION;
end if ;
for I in A'RANGE loop
C (I) := A (I) - B (I) ;
end loop ;
return C ;
end "-";
---
function "-" (A : MATRIX; B : MATRIX) return MATRIX is
C : MATRIX (A'FIRST (1) .. A'LAST (1), A'FIRST (2) .. A'LAST (2)) ;
-- ******************************************************************
-- This function performs the subtraction of matrix B from matrix A
-- resulting in a matrix. Comparability of dimensions is checked.
-- ******************************************************************
begin
if (A'FIRST (1) /= B'FIRST (1) or A'LAST (1) /= B'LAST (1))
or (A'FIRST (2) /= B'FIRST (2) or A'LAST (2) /= B'LAST (2)) then
raise INCOMPARABLE_DIMENSION;
end if ;
for I in A'RANGE (1) loop
for J in A'RANGE (2) loop
C (I, J) := A (I, J) - B (I, J) ;
end loop ;
end loop ;
return C ;
end "-";
---
function "*" (A : ELEMENT; B : VECTOR) return VECTOR is
C : VECTOR (B'FIRST .. B'LAST);
-- ******************************************************************
-- This function performs the scalar multiplication of a floating
-- number A and a vector B resulting in a vector.
-- ******************************************************************
begin
for I in B'RANGE loop
C (I) := A * B (I);
end loop;
return C ;
end "*";
---
function "*" (A : VECTOR; B : ELEMENT) return VECTOR is
begin
-- ********************************************************************
-- This function performs the scalar multiplication of a vector A and
-- a floating number B resulting in a vector.
-- ********************************************************************
return B * A;
end "*";
---
function "*" (A : VECTOR; B : VECTOR) return ELEMENT is
S : ELEMENT := 0.0;
-- *******************************************************************
-- This function performs the inner (dot) product of two vectors A
-- and B resulting in a floating number.
-- Comparability of dimensions is checked.
-- *******************************************************************
begin
if A'FIRST /= B'FIRST or A'LAST /= B'LAST then
raise INCOMPARABLE_DIMENSION;
end if ;
for I in A'RANGE loop
S := S + A (I) * B (I) ;
end loop ;
return S ;
end "*";
---
function "*" (A : MATRIX; B : VECTOR) return VECTOR is
C : VECTOR (A'FIRST (1) .. A'LAST (1));
SUM : ELEMENT;
-- ********************************************************************
-- This function performs the multiplication of a matrix A and a column
-- vector B resulting in a column vector.
-- Comparability of dimensions is checked.
-- ********************************************************************
-- **
begin
if A'FIRST (2) /= B'FIRST or A'LAST (2) /= B'LAST then
raise INCOMPARABLE_DIMENSION;
end if ;
for I in A'RANGE (1) loop
SUM := 0.0 ;
for K in A'RANGE (2) loop
SUM := SUM + A (I, K) * B (K);
end loop;
C (I) := SUM;
end loop ;
return C ;
end "*";
---
function MAT4MULT (UL : MATRIX; UR : MATRIX; BL : MATRIX; BR : MATRIX; B : VECTOR)
return VECTOR is
C : VECTOR (B'FIRST .. B'LAST);
SUMT, SUMB : ELEMENT;
-- ********************************************************************
-- This function performs the multiplication of a matrix A, broken into
-- 4 smaller ones due to memory limitations, and a column vector B
-- resulting in a column vector. Comparability of dimensions is
-- partially checked.
-- ********************************************************************
begin
if UL'LENGTH (1) /= UR'LENGTH (1) or
UL'LENGTH (1) /= BL'LENGTH (1) or
UL'LENGTH (1) /= BR'LENGTH (1) or
UL'LENGTH (2) /= UR'LENGTH (2) or
UL'LENGTH (2) /= BL'LENGTH (2) or
UL'LENGTH (2) /= BR'LENGTH (2) or
UL'LENGTH (2) /= UL'LENGTH (1) then
raise INCOMPARABLE_DIMENSION;
end if;
if UL'FIRST (1) /= B'FIRST or UL'LAST (1) /= B'LAST / 2 then
raise INCOMPARABLE_DIMENSION;
end if;
if UL'FIRST (2) /= B'FIRST or
UL'LAST (2) /= B'LAST / 2 or
UL'FIRST (1) /= B'FIRST or
UL'LAST (1) /= B'LAST / 2 or
UL'LAST (1) /= UL'LAST (2)
then
raise INCOMPARABLE_DIMENSION;
end if ;
for I in UL'RANGE (1) loop
SUMT := 0.0 ;
SUMB := 0.0 ;
for K in UL'RANGE (2) loop
SUMT := SUMT + UL (I, K) * B (K) + UR (I, K) * B (K + UL'LAST (2 ));
SUMB := SUMB + BL (I, K) * B (K) + BR (I, K) * B (K + UL'LAST (2 ));
end loop;
C (I) := SUMT;
C (I + UL'LAST (2)) := SUMB;
end loop ;
return C ;
end MAT4MULT;
---
function "*" (A : VECTOR; B : MATRIX) return VECTOR is
C : VECTOR (B'FIRST (2) .. B'LAST (2));
SUM : ELEMENT;
-- ********************************************************************
-- This function performs the multiplication of a row vector A and a
-- matrix B resulting in a row vector.
-- Comparability of dimensions is checked.
-- ********************************************************************
begin
if A'FIRST /= B'FIRST (1) or A'LAST /= B'LAST (1) then
raise INCOMPARABLE_DIMENSION;
end if ;
for J in B'RANGE (2) loop
SUM := 0.0 ;
for K in A'RANGE loop
SUM := SUM + A (K) * B (K, J);
end loop;
C (J) := SUM;
end loop ;
return C ;
end "*";
---
function "*" (A : ELEMENT; B : MATRIX) return MATRIX is
C : MATRIX (B'FIRST (1) .. B'LAST (1), B'FIRST (2) .. B'LAST (2));
-- ********************************************************************
-- This function performs the scalar multipliction of a matrix B by
-- a floating number A resulting in a matrix.
-- ********************************************************************
begin
for I in B'RANGE (1) loop
for J in B'RANGE (2) loop
C (I, J) := A * B (I, J);
end loop ;
end loop ;
return C ;
end "*";
---
function "*" (A : MATRIX; B : ELEMENT) return MATRIX is
C : MATRIX (A'FIRST (1) .. A'LAST (1), A'FIRST (2) .. A'LAST (2));
-- *****************************************************************
-- This function performs the scalar multipliction of a matrix A
-- by a floating number B resulting in a matrix.
-- *****************************************************************
begin
return B * A ;
end "*";
---
function "*" (A : MATRIX; B : MATRIX) return MATRIX is
C : MATRIX (A'FIRST (1) .. A'LAST (1), B'FIRST (2) .. B'LAST (2));
SUM : ELEMENT;
-- ********************************************************************
-- This function performs the multiplication of matrix A and matrix B
-- resulting in a matrix. Comparability of dimensions is checked.
-- ********************************************************************
begin
if A'FIRST (2) /= B'FIRST (1) or A'LAST (2) /= B'LAST (1) then
raise INCOMPARABLE_DIMENSION;
end if ;
for I in A'RANGE (1) loop
for J in B'RANGE (2) loop
SUM := 0.0 ;
for K in A'RANGE (2) loop
SUM := SUM + A (I, K) * B (K, J);
end loop;
C (I, J) := SUM;
end loop ;
end loop ;
return C ;
end "*";
---
function "*" (A : ELEMENT; B : MATR2T) return MATR2T is
C : MATR2T (B'FIRST (1) .. B'LAST (1));
-- ********************************************************************
-- This function performs the multiplication of each element of a
-- MATR2T by a FLOAT resulting in a MATR2T.
-- ********************************************************************
begin
for I in B'RANGE (1) loop
C (I) := A * B (I);
end loop ;
return C ;
end "*";
---
function "*" (A : VEC2T; B : MATR2T) return VECTOR is
C : VECTOR (B'FIRST (1) .. B'LAST (1));
-- ********************************************************************
-- This function performs the DOT PRODUCT of each element of a
-- MATR2T by a VEC2T resulting in a VECTOR.
-- ********************************************************************
begin
for I in B'RANGE (1) loop
C (I) := A * B (I);
end loop ;
return C ;
end "*";
---
function "*" (A : VECTOR; B : MATR2T) return MATR2T is
TEMP : MATR2T (B'RANGE);
-- ********************************************************************
-- This function multiplies each VEC2T of a MATR2T by a float from a
-- the corresponding term of a VECTOR resulting in a MATR2T.
-- ********************************************************************
begin
if A'FIRST /= B'FIRST or A'LAST /= B'LAST then
raise INCOMPARABLE_DIMENSION;
end if;
for I in B'RANGE loop
TEMP (I) := A (I) * B (I);
end loop;
return TEMP;
end "*";
function "**" (A : MATRIX; P : INTEGER) return MATRIX is
B, C : MATRIX (A'FIRST (1) .. A'LAST (1), A'FIRST (1) .. A'LAST (1));
I_PIVOT, J_PIVOT : INDEX range A'FIRST (1) .. A'LAST (1);
BIG_ENTRY, TEMP, EPSILON : ELEMENT ;
L, M : array (A'RANGE (1)) of INDEX ;
-- *******************************************************************
-- This function performs the square matrix operation of " matrix A
-- raise to integer power P ". When P is negative , say P = -N ,
-- A**(-N) = (inverse(A))**N , that is, the inverse of A raise to
-- power N . In this case, matrix A must be non-singular.
-- Exceptions will be raised if the matrix A is not a square matrix,
-- or if matrix A is singular.
-- *******************************************************************
begin
if A'FIRST (1) /= A'FIRST (2) or A'LAST (1) /= A'LAST (2) then
-- if not a square matrix
raise INCOMPARABLE_DIMENSION ;
end if;
if P = 0 then
--& B = identity matrix
for I in A'RANGE (1) loop
for J in A'RANGE (1) loop
if I /= J then
B (I, J) := 0.0;
else
B (I, J) := 1.0;
end if;
end loop;
end loop;
return B;
end if ;
B := A ;
if P > 0 then
--& B = A multiplied itself for P times
for I in 1 .. P - 1 loop
B := B * A ;
end loop ;
return B ;
end if;
-- P is negative, find inverse first
-- initiate the row and column interchange information
for K in B'RANGE (1) loop
L (K) := K ; -- row interchage information
M (K) := K ; -- column interchange information
end loop;
-- major loop for inverse
for K in B'RANGE (1) loop
-- & search for row and column index I_PIVOT, J_PIVOT
-- & both in (K .. B'LAST(1) ) for maximum B(I,J)
-- & in absolute value :BIG_ENTRY
BIG_ENTRY := 0.0 ;
--
-- check matrix singularity
--
for I in K .. B'LAST (1) loop
for J in K .. B'LAST (1) loop
if abs (B (I, J)) > abs (BIG_ENTRY) then
BIG_ENTRY := B (I, J) ;
I_PIVOT := I ;
J_PIVOT := J ;
end if;
end loop;
end loop;
if K = B'FIRST (1) then
if BIG_ENTRY = 0.0 then
raise SINGULAR;
else
EPSILON := ELEMENT (A'LENGTH (1)) * abs (BIG_ENTRY) * 0.000001;
end if;
else
if abs (BIG_ENTRY) < EPSILON then
raise SINGULAR ;
end if;
end if;
-- interchange row and column
--& interchange K-th and I_PIVOT-th rows
if I_PIVOT /= K then
for J in B'RANGE (1) loop
TEMP := B (I_PIVOT, J);
B (I_PIVOT, J) := B (K, J) ;
B (K, J) := TEMP ;
end loop;
L (K) := I_PIVOT ;
end if;
--& interchange K-th and J_PIVOT-th columns
if J_PIVOT /= K then
for I in B'RANGE (1) loop
TEMP := B (I, J_PIVOT) ;
B (I, J_PIVOT) := B (I, K) ;
B (I, K) := TEMP ;
end loop ;
M (K) := J_PIVOT ;
end if ;
--& divide K-th column by minus pivot (-BIG_ENTRY)
for I in B'RANGE (1) loop
if I /= K then
B (I, K) := B (I, K) / ( - BIG_ENTRY) ;
end if;
end loop ;
-- reduce matrix row by row
for I in B'RANGE (1) loop
if I /= K then
for J in B'RANGE (1) loop
if J /= K then
B (I, J) := B (I, J) + B (I, K) * B (K, J);
end if ;
end loop ;
end if ;
end loop ;
--& divide K-th row by pivot
for J in B'RANGE (1) loop
if J /= K then
B (K, J) := B (K, J) / BIG_ENTRY ;
end if ;
end loop ;
B (K, K) := 1.0 / BIG_ENTRY ;
end loop ; -- end of major inverse loop
-- final column and row interchange to obtain
-- inverse of A, i.e. A**(-1)
for K in reverse B'RANGE (1) loop
-- column interchage
J_PIVOT := L (K) ;
if J_PIVOT /= K then
--& intechange B(I,J_PIVOT) and B(I,K) for each row I
for I in B'RANGE (1) loop
TEMP := B (I, J_PIVOT) ;
B (I, J_PIVOT) := B (I, K) ;
B (I, K) := TEMP ;
end loop ;
end if ;
-- row interchage
I_PIVOT := M (K) ;
if I_PIVOT /= K then
--& INTECHANGE B(I_PIVOT,J) and B(K,J) for each column J
for J in B'RANGE (1) loop
TEMP := B (I_PIVOT, J) ;
B (I_PIVOT, J) := B (K, J) ;
B (K, J) := TEMP ;
end loop ;
end if ;
end loop ;
-- inverse of A is obtained and stored in B
-- now ready to handle the negative power
-- & C = B**(-P)
if P = - 1 then
return B ;
end if ;
C := B ;
for I in P + 1 .. - 1 loop
C := C * B ;
end loop ;
return C;
end "**" ;
---
function "**" (A : VECTOR; B : VECTOR) return VECTOR is
VTEMP : VECTOR (1 .. 3);
-- *******************************************************************
-- This function performs the cross product of two vectors A
-- and B resulting in a VECTOR. Usage, C := A ** B;
-- Comparability of dimensions is checked. LIMITED TO 3D.
-- *******************************************************************
begin
if A'FIRST /= B'FIRST or A'LAST /= B'LAST then
raise INCOMPARABLE_DIMENSION;
end if ;
VTEMP (1) := A (2) * B (3) - A (3) * B (2);
VTEMP (2) := A (3) * B (1) - A (1) * B (3);
VTEMP (3) := A (1) * B (2) - A (2) * B (1);
return VTEMP ;
end "**";
---
function JCROSS (A : VEC2T) return VEC2T is
--*********************************************************************
-- This function rotates a Vec2T 90 degrees cw.
--*********************************************************************
VTEMP : VEC2T;
begin
VTEMP := (A (2), (( - 1.0) * A (1)));
return VTEMP;
end JCROSS;
---
function JCROSS (A : MATR2T) return MATR2T is
--*********************************************************************
-- This function rotates each component Vec2T of MATR2T 90 degrees cw.
--*********************************************************************
B : MATR2T (A'FIRST (1) .. A'LAST (1));
begin
for I in A'RANGE loop
B (I) := JCROSS (A (I));
end loop;
return B;
end JCROSS;
---
function ROTX (A : VEC2T) return VEC2T is
--*********************************************************************
-- This function rotates a Vec2T 180 degrees about the X axis.
--*********************************************************************
begin
return (A (1), - A (2));
end ROTX;
---
function ROTY (A : VEC2T) return VEC2T is
--*********************************************************************
-- This function rotates a Vec2T 180 degrees about the Y axis.
--*********************************************************************
begin
return ( - A (1), A (2));
end ROTY;
---
function AXBDOTJ (A : VEC2T; B : VEC2T) return ELEMENT is
--*********************************************************************
--Gets magnitude of A cross B for 2 2D vectors.
--*********************************************************************
TEMP : ELEMENT;
begin
TEMP := A (2) * B (1) - A (1) * B (2);
return TEMP;
end AXBDOTJ;
---
function GETTAN (A : VEC2T; B : VEC2T) return ELEMENT is
--*********************************************************************
--Gets TAN(THETA) where THETA is CW angle between 2 2D vectors.
--*********************************************************************
EPSILON, NUM, DENOM : ELEMENT := 0.00000001;
begin
DENOM := A * B;
NUM := AXBDOTJ (A, B);
if DENOM < EPSILON and DENOM >= 0.0 then
PUT_LINE ("Tangent is beyond the limit val of");
return (NUM / EPSILON);
elsif DENOM > - EPSILON and DENOM < 0.0 then
PUT_LINE ("Tangent is beyond the limit val of");
return (( - NUM) / EPSILON);
else
return AXBDOTJ (A, B) / (A * B);
end if;
end GETTAN;
end MATRIX_PACKAGE;