[occt.git] / src / gp / gp_Mat.cxx

// Copyright (c) 1995-1999 Matra Datavision
// Copyright (c) 1999-2014 OPEN CASCADE SAS
//
// This file is part of Open CASCADE Technology software library.
//
// This library is free software; you can redistribute it and/or modify it under
// the terms of the GNU Lesser General Public License version 2.1 as published
// by the Free Software Foundation, with special exception defined in the file
// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
// distribution for complete text of the license and disclaimer of any warranty.
//
// Alternatively, this file may be used under the terms of Open CASCADE
// commercial license or contractual agreement.

//  10/09/97 : PMN : Correction BUC40192 (pb avec les matrices negatives)

#ifndef OCCT_DEBUG
#define No_Standard_OutOfRange
#define No_Standard_ConstructionError
#endif


#include <gp_GTrsf.hxx>
#include <gp_Mat.hxx>
#include <gp_Trsf.hxx>
#include <gp_XYZ.hxx>
#include <Standard_ConstructionError.hxx>
#include <Standard_OutOfRange.hxx>

#define M00 ((Standard_Real*)M)[0]
#define M01 ((Standard_Real*)M)[1]
#define M02 ((Standard_Real*)M)[2]
#define M10 ((Standard_Real*)M)[3]
#define M11 ((Standard_Real*)M)[4]
#define M12 ((Standard_Real*)M)[5]
#define M20 ((Standard_Real*)M)[6]
#define M21 ((Standard_Real*)M)[7]
#define M22 ((Standard_Real*)M)[8]

#define N00 ((Standard_Real*)N)[0]
#define N01 ((Standard_Real*)N)[1]
#define N02 ((Standard_Real*)N)[2]
#define N10 ((Standard_Real*)N)[3]
#define N11 ((Standard_Real*)N)[4]
#define N12 ((Standard_Real*)N)[5]
#define N20 ((Standard_Real*)N)[6]
#define N21 ((Standard_Real*)N)[7]
#define N22 ((Standard_Real*)N)[8]

gp_Mat::gp_Mat (const gp_XYZ& Col1,
		const gp_XYZ& Col2,
		const gp_XYZ& Col3)
{
  const Standard_Address M = (Standard_Address)&(matrix[0][0]);
  M00 = Col1.X(); M10 = Col1.Y(); M20 = Col1.Z();
  M01 = Col2.X(); M11 = Col2.Y(); M21 = Col2.Z();
  M02 = Col3.X(); M12 = Col3.Y(); M22 = Col3.Z();
}

void gp_Mat::SetCol (const Standard_Integer Col,
		     const gp_XYZ& Value) {

  Standard_OutOfRange_Raise_if (Col < 1 || Col > 3, " ");
  const Standard_Address M = (Standard_Address)&(matrix[0][0]);
  if      (Col == 1) {
    M00 = Value.X(); M10 = Value.Y(); M20 = Value.Z();
  }
  else if (Col == 2) {
    M01 = Value.X(); M11 = Value.Y(); M21 = Value.Z();
  }
  else {
    M02 = Value.X(); M12 = Value.Y(); M22 = Value.Z();
  }
}

void gp_Mat::SetCols (const gp_XYZ& Col1,
		      const gp_XYZ& Col2,
		      const gp_XYZ& Col3)
{
  Mat00 = Col1.X(); Mat10 = Col1.Y(); Mat20 = Col1.Z();
  Mat01 = Col2.X(); Mat11 = Col2.Y(); Mat21 = Col2.Z();
  Mat02 = Col3.X(); Mat12 = Col3.Y(); Mat22 = Col3.Z();
}

void gp_Mat::SetCross (const gp_XYZ& Ref)
{
  const Standard_Address M = (Standard_Address)&(matrix[0][0]);
  Standard_Real X = Ref.X();
  Standard_Real Y = Ref.Y();
  Standard_Real Z = Ref.Z();
  M00 = M11 = M22 = 0.0;
  M01 = - Z;  
  M02 = Y;
  M12 = - X;
  M10 = Z;
  M20 = - Y;
  M21 = X;
}

void gp_Mat::SetDot (const gp_XYZ& Ref)
{
  const Standard_Address M = (Standard_Address)&(matrix[0][0]);
  Standard_Real X = Ref.X();
  Standard_Real Y = Ref.Y();
  Standard_Real Z = Ref.Z();
  M00 = X * X;
  M11 = Y * Y;
  M22 = Z * Z;
  M01 = X * Y;
  M02 = X * Z;
  M12 = Y * Z;
  M10 = M01;
  M20 = M02;
  M21 = M12;
}

void gp_Mat::SetRotation (const gp_XYZ& Axis,
			  const Standard_Real Ang)
{
  //    Rot = I + sin(Ang) * M + (1. - cos(Ang)) * M*M
  //    avec  M . XYZ = Axis ^ XYZ

//  const Standard_Address M = (Standard_Address)&(matrix[0][0]);
  gp_XYZ V = Axis.Normalized();
  SetCross (V);
  Multiply (sin(Ang));
  gp_Mat Temp;
  Temp.SetScale (1.0);
  Add (Temp);
  Standard_Real A = V.X();
  Standard_Real B = V.Y();
  Standard_Real C = V.Z();
  Temp.SetRow (1, gp_XYZ(- C*C - B*B,      A*B,           A*C     ));
  Temp.SetRow (2, gp_XYZ(     A*B,      -A*A - C*C,        B*C    ));
  Temp.SetRow (3, gp_XYZ(     A*C,          B*C,       - A*A - B*B));
  Temp.Multiply (1.0 - cos(Ang));
  Add (Temp);
}

void gp_Mat::SetRow (const Standard_Integer Row,
		     const gp_XYZ& Value)
{
  Standard_OutOfRange_Raise_if (Row < 1 || Row > 3, " ");
  const Standard_Address M = (Standard_Address)&(matrix[0][0]);
  if      (Row == 1) {
    M00 = Value.X(); M01 = Value.Y(); M02 = Value.Z();
  }
  else if (Row == 2) {
    M10 = Value.X(); M11 = Value.Y(); M12 = Value.Z();
  }
  else {
    M20 = Value.X(); M21 = Value.Y(); M22 = Value.Z();
  }
}

void gp_Mat::SetRows (const gp_XYZ& Row1,
		      const gp_XYZ& Row2,
		      const gp_XYZ& Row3)
{
  const Standard_Address M = (Standard_Address)&(matrix[0][0]);
  M00 = Row1.X(); M01 = Row1.Y(); M02 = Row1.Z();
  M10 = Row2.X(); M11 = Row2.Y(); M12 = Row2.Z();
  M20 = Row3.X(); M21 = Row3.Y(); M22 = Row3.Z();
}

gp_XYZ gp_Mat::Column (const Standard_Integer Col) const
{
  Standard_OutOfRange_Raise_if (Col < 1 || Col > 3, "gp_Mat::Column() - wrong index");
  const Standard_Address M = (Standard_Address)&(matrix[0][0]);
  if (Col == 1) return gp_XYZ (M00,M10,M20);
  if (Col == 2) return gp_XYZ (M01,M11,M21);
  return gp_XYZ (M02,M12,M22);
}

gp_XYZ gp_Mat::Diagonal () const
{
  const Standard_Address M = (Standard_Address)&(matrix[0][0]);
  return gp_XYZ (M00, M11, M22);
}

gp_XYZ gp_Mat::Row (const Standard_Integer Row) const
{
  Standard_OutOfRange_Raise_if (Row < 1 || Row > 3, "gp_Mat::Row() - wrong index");
  const Standard_Address M = (Standard_Address)&(matrix[0][0]);
  if (Row == 1) return gp_XYZ (M00,M01,M02);
  if (Row == 2) return gp_XYZ (M10,M11,M12);
  return gp_XYZ (M20,M21,M22);
}

void gp_Mat::Invert ()
{ 
  Standard_Real new_array[3][3] ;
  const Standard_Address M = (Standard_Address)&(   matrix[0][0]);
  const Standard_Address N = (Standard_Address)&(new_array[0][0]);  

  //
  // calcul de  la transposee de la commatrice
  //
  N00 = M11 * M22 - M12 * M21 ;
  N10 = -(M10 * M22 - M20 * M12) ;
  N20 = M10 * M21 - M20 * M11 ;
  N01 = - (M01 * M22 - M21 * M02) ;
  N11 = M00 * M22 - M20 * M02 ;
  N21 = -(M00 * M21 - M20 * M01) ;
  N02 = M01 * M12 - M11 * M02 ;
  N12 = -(M00 * M12 - M10 * M02) ;
  N22 = M00 * M11 - M01 * M10 ;
  Standard_Real det =  M00 * N00 + M01* N10 + M02 * N20 ;
  Standard_Real val = det;
  if (val < 0) val = - val;
  Standard_ConstructionError_Raise_if (val <= gp::Resolution(), "gp_Mat::Invert() - matrix has zero determinant");
  det = 1.0e0 / det ;
  M00 = N00;
  M10 = N10;
  M20 = N20;
  M01 = N01;
  M11 = N11;
  M21 = N21;
  M02 = N02;
  M12 = N12;
  M22 = N22;
  Multiply(det) ;
}

gp_Mat gp_Mat::Inverted () const
{ 
  gp_Mat NewMat;
  const Standard_Address M = (Standard_Address)&(       matrix[0][0]);
  const Standard_Address N = (Standard_Address)&(NewMat.matrix[0][0]);
  //
  // calcul de  la transposee de la commatrice
  //
  N00 = M11 * M22 - M12 * M21 ;
  N10 = -(M10 * M22 - M20 * M12) ;
  N20 = M10 * M21 - M20 * M11 ;
  N01 = - (M01 * M22 - M21 * M02) ;
  N11 = M00 * M22 - M20 * M02 ;
  N21 = -(M00 * M21 - M20 * M01) ;
  N02 = M01 * M12 - M11 * M02 ;
  N12 = -(M00 * M12 - M10 * M02) ;
  N22 = M00 * M11 - M01 * M10 ;
  Standard_Real det =  M00 * N00 + M01* N10 + M02 * N20 ;
  Standard_Real val = det;
  if (val < 0) val = - val;
  Standard_ConstructionError_Raise_if (val <= gp::Resolution(), "gp_Mat::Inverted() - matrix has zero determinant");
  det = 1.0e0 / det ;
  NewMat.Multiply(det) ;
  return NewMat;
}

void gp_Mat::Power (const Standard_Integer N)
{
  if (N == 1) { }
  else if (N == 0)  { SetIdentity() ; }
  else if (N == -1) { Invert(); }
  else {
    if (N < 0) { Invert(); }
    Standard_Integer Npower = N;
    if (Npower < 0) Npower = - Npower;
    Npower--;
    gp_Mat Temp = *this;
    for(;;) {
      if (IsOdd(Npower)) Multiply (Temp);
      if (Npower == 1)   break; 
      Temp.Multiply (Temp);
      Npower>>=1;
    }
  }
}
Commit	Line	Data
b311480e	1	// Copyright (c) 1995-1999 Matra Datavision
973c2be1	2	// Copyright (c) 1999-2014 OPEN CASCADE SAS
b311480e	3	//
973c2be1	4	// This file is part of Open CASCADE Technology software library.
b311480e	5	//
d5f74e42	6	// This library is free software; you can redistribute it and/or modify it under
d5f74e42	7	// the terms of the GNU Lesser General Public License version 2.1 as published
973c2be1	8	// by the Free Software Foundation, with special exception defined in the file
	9	// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
	10	// distribution for complete text of the license and disclaimer of any warranty.
b311480e	11	//
973c2be1	12	// Alternatively, this file may be used under the terms of Open CASCADE
973c2be1	13	// commercial license or contractual agreement.
b311480e	14
7fd59977	15	// 10/09/97 : PMN : Correction BUC40192 (pb avec les matrices negatives)
7fd59977	16
0797d9d3	17	#ifndef OCCT_DEBUG
7fd59977	18	#define No_Standard_OutOfRange
	19	#define No_Standard_ConstructionError
	20	#endif
	21
42cf5bc1	22
	23	#include <gp_GTrsf.hxx>
	24	#include <gp_Mat.hxx>
	25	#include <gp_Trsf.hxx>
	26	#include <gp_XYZ.hxx>
	27	#include <Standard_ConstructionError.hxx>
	28	#include <Standard_OutOfRange.hxx>
7fd59977	29
	30	#define M00 ((Standard_Real*)M)[0]
	31	#define M01 ((Standard_Real*)M)[1]
	32	#define M02 ((Standard_Real*)M)[2]
	33	#define M10 ((Standard_Real*)M)[3]
	34	#define M11 ((Standard_Real*)M)[4]
	35	#define M12 ((Standard_Real*)M)[5]
	36	#define M20 ((Standard_Real*)M)[6]
	37	#define M21 ((Standard_Real*)M)[7]
	38	#define M22 ((Standard_Real*)M)[8]
	39
	40	#define N00 ((Standard_Real*)N)[0]
	41	#define N01 ((Standard_Real*)N)[1]
	42	#define N02 ((Standard_Real*)N)[2]
	43	#define N10 ((Standard_Real*)N)[3]
	44	#define N11 ((Standard_Real*)N)[4]
	45	#define N12 ((Standard_Real*)N)[5]
	46	#define N20 ((Standard_Real*)N)[6]
	47	#define N21 ((Standard_Real*)N)[7]
	48	#define N22 ((Standard_Real*)N)[8]
	49
	50	gp_Mat::gp_Mat (const gp_XYZ& Col1,
	51	const gp_XYZ& Col2,
	52	const gp_XYZ& Col3)
	53	{
	54	const Standard_Address M = (Standard_Address)&(matrix[0][0]);
	55	M00 = Col1.X(); M10 = Col1.Y(); M20 = Col1.Z();
	56	M01 = Col2.X(); M11 = Col2.Y(); M21 = Col2.Z();
	57	M02 = Col3.X(); M12 = Col3.Y(); M22 = Col3.Z();
	58	}
	59
	60	void gp_Mat::SetCol (const Standard_Integer Col,
	61	const gp_XYZ& Value) {
	62
	63	Standard_OutOfRange_Raise_if (Col < 1 \|\| Col > 3, " ");
	64	const Standard_Address M = (Standard_Address)&(matrix[0][0]);
	65	if (Col == 1) {
	66	M00 = Value.X(); M10 = Value.Y(); M20 = Value.Z();
	67	}
	68	else if (Col == 2) {
	69	M01 = Value.X(); M11 = Value.Y(); M21 = Value.Z();
	70	}
	71	else {
	72	M02 = Value.X(); M12 = Value.Y(); M22 = Value.Z();
	73	}
	74	}
	75
	76	void gp_Mat::SetCols (const gp_XYZ& Col1,
	77	const gp_XYZ& Col2,
	78	const gp_XYZ& Col3)
	79	{
7fd59977	80	Mat00 = Col1.X(); Mat10 = Col1.Y(); Mat20 = Col1.Z();
	81	Mat01 = Col2.X(); Mat11 = Col2.Y(); Mat21 = Col2.Z();
	82	Mat02 = Col3.X(); Mat12 = Col3.Y(); Mat22 = Col3.Z();
	83	}
	84
	85	void gp_Mat::SetCross (const gp_XYZ& Ref)
	86	{
	87	const Standard_Address M = (Standard_Address)&(matrix[0][0]);
	88	Standard_Real X = Ref.X();
	89	Standard_Real Y = Ref.Y();
	90	Standard_Real Z = Ref.Z();
	91	M00 = M11 = M22 = 0.0;
	92	M01 = - Z;
	93	M02 = Y;
	94	M12 = - X;
	95	M10 = Z;
	96	M20 = - Y;
	97	M21 = X;
	98	}
	99
	100	void gp_Mat::SetDot (const gp_XYZ& Ref)
	101	{
	102	const Standard_Address M = (Standard_Address)&(matrix[0][0]);
	103	Standard_Real X = Ref.X();
	104	Standard_Real Y = Ref.Y();
	105	Standard_Real Z = Ref.Z();
	106	M00 = X * X;
	107	M11 = Y * Y;
	108	M22 = Z * Z;
	109	M01 = X * Y;
	110	M02 = X * Z;
	111	M12 = Y * Z;
	112	M10 = M01;
	113	M20 = M02;
	114	M21 = M12;
	115	}
	116
	117	void gp_Mat::SetRotation (const gp_XYZ& Axis,
	118	const Standard_Real Ang)
	119	{
	120	// Rot = I + sin(Ang) * M + (1. - cos(Ang)) * M*M
	121	// avec M . XYZ = Axis ^ XYZ
	122
	123	// const Standard_Address M = (Standard_Address)&(matrix[0][0]);
	124	gp_XYZ V = Axis.Normalized();
	125	SetCross (V);
	126	Multiply (sin(Ang));
	127	gp_Mat Temp;
	128	Temp.SetScale (1.0);
	129	Add (Temp);
	130	Standard_Real A = V.X();
	131	Standard_Real B = V.Y();
	132	Standard_Real C = V.Z();
	133	Temp.SetRow (1, gp_XYZ(- CC - BB, AB, AC ));
	134	Temp.SetRow (2, gp_XYZ( AB, -AA - CC, BC ));
	135	Temp.SetRow (3, gp_XYZ( AC, BC, - AA - BB));
	136	Temp.Multiply (1.0 - cos(Ang));
	137	Add (Temp);
	138	}
	139
	140	void gp_Mat::SetRow (const Standard_Integer Row,
	141	const gp_XYZ& Value)
	142	{
	143	Standard_OutOfRange_Raise_if (Row < 1 \|\| Row > 3, " ");
144	const Standard_Address M = (Standard_Address)&(matrix[0][0]);
145	if (Row == 1) {
146	M00 = Value.X(); M01 = Value.Y(); M02 = Value.Z();
147	}
148	else if (Row == 2) {
149	M10 = Value.X(); M11 = Value.Y(); M12 = Value.Z();
150	}
151	else {
152	M20 = Value.X(); M21 = Value.Y(); M22 = Value.Z();
153	}
154	}
155
156	void gp_Mat::SetRows (const gp_XYZ& Row1,
157	const gp_XYZ& Row2,
158	const gp_XYZ& Row3)
159	{
160	const Standard_Address M = (Standard_Address)&(matrix[0][0]);
161	M00 = Row1.X(); M01 = Row1.Y(); M02 = Row1.Z();
162	M10 = Row2.X(); M11 = Row2.Y(); M12 = Row2.Z();
163	M20 = Row3.X(); M21 = Row3.Y(); M22 = Row3.Z();
164	}
165
166	gp_XYZ gp_Mat::Column (const Standard_Integer Col) const
167	{
2d2b3d53	168	Standard_OutOfRange_Raise_if (Col < 1 \|\| Col > 3, "gp_Mat::Column() - wrong index");
7fd59977	169	const Standard_Address M = (Standard_Address)&(matrix[0][0]);
	170	if (Col == 1) return gp_XYZ (M00,M10,M20);
	171	if (Col == 2) return gp_XYZ (M01,M11,M21);
	172	return gp_XYZ (M02,M12,M22);
	173	}
	174
	175	gp_XYZ gp_Mat::Diagonal () const
	176	{
	177	const Standard_Address M = (Standard_Address)&(matrix[0][0]);
	178	return gp_XYZ (M00, M11, M22);
	179	}
	180
	181	gp_XYZ gp_Mat::Row (const Standard_Integer Row) const
	182	{
2d2b3d53	183	Standard_OutOfRange_Raise_if (Row < 1 \|\| Row > 3, "gp_Mat::Row() - wrong index");
7fd59977	184	const Standard_Address M = (Standard_Address)&(matrix[0][0]);
	185	if (Row == 1) return gp_XYZ (M00,M01,M02);
	186	if (Row == 2) return gp_XYZ (M10,M11,M12);
	187	return gp_XYZ (M20,M21,M22);
	188	}
	189
	190	void gp_Mat::Invert ()
	191	{
	192	Standard_Real new_array[3][3] ;
	193	const Standard_Address M = (Standard_Address)&( matrix[0][0]);
	194	const Standard_Address N = (Standard_Address)&(new_array[0][0]);
	195
	196	//
	197	// calcul de la transposee de la commatrice
	198	//
	199	N00 = M11 * M22 - M12 * M21 ;
	200	N10 = -(M10 * M22 - M20 * M12) ;
	201	N20 = M10 * M21 - M20 * M11 ;
	202	N01 = - (M01 * M22 - M21 * M02) ;
	203	N11 = M00 * M22 - M20 * M02 ;
	204	N21 = -(M00 * M21 - M20 * M01) ;
	205	N02 = M01 * M12 - M11 * M02 ;
	206	N12 = -(M00 * M12 - M10 * M02) ;
	207	N22 = M00 * M11 - M01 * M10 ;
	208	Standard_Real det = M00 * N00 + M01* N10 + M02 * N20 ;
	209	Standard_Real val = det;
	210	if (val < 0) val = - val;
2d2b3d53	211	Standard_ConstructionError_Raise_if (val <= gp::Resolution(), "gp_Mat::Invert() - matrix has zero determinant");
7fd59977	212	det = 1.0e0 / det ;
	213	M00 = N00;
	214	M10 = N10;
	215	M20 = N20;
	216	M01 = N01;
	217	M11 = N11;
	218	M21 = N21;
	219	M02 = N02;
	220	M12 = N12;
	221	M22 = N22;
	222	Multiply(det) ;
	223	}
	224
	225	gp_Mat gp_Mat::Inverted () const
	226	{
	227	gp_Mat NewMat;
	228	const Standard_Address M = (Standard_Address)&( matrix[0][0]);
	229	const Standard_Address N = (Standard_Address)&(NewMat.matrix[0][0]);
	230	//
	231	// calcul de la transposee de la commatrice
	232	//
	233	N00 = M11 * M22 - M12 * M21 ;
	234	N10 = -(M10 * M22 - M20 * M12) ;
	235	N20 = M10 * M21 - M20 * M11 ;
	236	N01 = - (M01 * M22 - M21 * M02) ;
	237	N11 = M00 * M22 - M20 * M02 ;
	238	N21 = -(M00 * M21 - M20 * M01) ;
	239	N02 = M01 * M12 - M11 * M02 ;
	240	N12 = -(M00 * M12 - M10 * M02) ;
	241	N22 = M00 * M11 - M01 * M10 ;
	242	Standard_Real det = M00 * N00 + M01* N10 + M02 * N20 ;
	243	Standard_Real val = det;
	244	if (val < 0) val = - val;
2d2b3d53	245	Standard_ConstructionError_Raise_if (val <= gp::Resolution(), "gp_Mat::Inverted() - matrix has zero determinant");
7fd59977	246	det = 1.0e0 / det ;
	247	NewMat.Multiply(det) ;
	248	return NewMat;
	249	}
	250
	251	void gp_Mat::Power (const Standard_Integer N)
	252	{
	253	if (N == 1) { }
	254	else if (N == 0) { SetIdentity() ; }
	255	else if (N == -1) { Invert(); }
	256	else {
	257	if (N < 0) { Invert(); }
	258	Standard_Integer Npower = N;
	259	if (Npower < 0) Npower = - Npower;
	260	Npower--;
	261	gp_Mat Temp = *this;
302f96fb	262	for(;;) {
7fd59977	263	if (IsOdd(Npower)) Multiply (Temp);
	264	if (Npower == 1) break;
	265	Temp.Multiply (Temp);
	266	Npower>>=1;
	267	}
	268	}
	269	}
	270