24_7 Cross Product

Input:

v1 : ARRAY [0..2] OF LREAL;
v1 : ARRAY [0..2] OF LREAL;

Output:

cross : ARRAY [0..2] OF LREAL;

Type: Function

Responsible: Magnus

Simple helper function for 24_7 Forward Kinematics. Concept taken from wikipedia.

Code Overview

FUNCTION cross : ARRAY [0..2] OF LREAL
VAR_INPUT
	v1 : ARRAY [0..2] OF LREAL;
	v2 : ARRAY [0..2] OF LREAL;
END_VAR
VAR
	a1 : LREAL := v1[0];
	a2 : LREAL := v1[1];
	a3 : LREAL := v1[2];
	b1 : LREAL := v2[0];
	b2 : LREAL := v2[1];
	b3 : LREAL := v2[2];
	
	tempVec : ARRAY [0..2] OF LREAL;
END_VAR

tempVec[0] := (a2*b3-a3*b2);
tempVec[1] := (a3*b1-a1*b3);
tempVec[2] := (a1*b2-a2*b1);
cross := tempVec;

Space shortcuts

Page tree