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

// File gp_Mat.cxx, JCV 05/12/90
//  10/09/97 : PMN : Correction BUC40192 (pb avec les matrices negatives)

#ifndef DEB
#define No_Standard_OutOfRange
#define No_Standard_ConstructionError
#endif

#include <gp_Mat.ixx>

#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, "");
  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, "");
  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(),"");
  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(),"");
  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;
    while (1) {
      if (IsOdd(Npower)) Multiply (Temp);
      if (Npower == 1)   break; 
      Temp.Multiply (Temp);
      Npower>>=1;
    }
  }
}
Commit	Line	Data
7fd59977	1	// File gp_Mat.cxx, JCV 05/12/90
	2	// 10/09/97 : PMN : Correction BUC40192 (pb avec les matrices negatives)
	3
	4	#ifndef DEB
	5	#define No_Standard_OutOfRange
	6	#define No_Standard_ConstructionError
	7	#endif
	8
	9	#include <gp_Mat.ixx>
	10
	11	#define M00 ((Standard_Real*)M)[0]
	12	#define M01 ((Standard_Real*)M)[1]
	13	#define M02 ((Standard_Real*)M)[2]
	14	#define M10 ((Standard_Real*)M)[3]
	15	#define M11 ((Standard_Real*)M)[4]
	16	#define M12 ((Standard_Real*)M)[5]
	17	#define M20 ((Standard_Real*)M)[6]
	18	#define M21 ((Standard_Real*)M)[7]
	19	#define M22 ((Standard_Real*)M)[8]
	20
	21	#define N00 ((Standard_Real*)N)[0]
	22	#define N01 ((Standard_Real*)N)[1]
	23	#define N02 ((Standard_Real*)N)[2]
	24	#define N10 ((Standard_Real*)N)[3]
	25	#define N11 ((Standard_Real*)N)[4]
	26	#define N12 ((Standard_Real*)N)[5]
	27	#define N20 ((Standard_Real*)N)[6]
	28	#define N21 ((Standard_Real*)N)[7]
	29	#define N22 ((Standard_Real*)N)[8]
	30
	31	gp_Mat::gp_Mat (const gp_XYZ& Col1,
	32	const gp_XYZ& Col2,
	33	const gp_XYZ& Col3)
	34	{
	35	const Standard_Address M = (Standard_Address)&(matrix[0][0]);
	36	M00 = Col1.X(); M10 = Col1.Y(); M20 = Col1.Z();
	37	M01 = Col2.X(); M11 = Col2.Y(); M21 = Col2.Z();
	38	M02 = Col3.X(); M12 = Col3.Y(); M22 = Col3.Z();
	39	}
	40
	41	void gp_Mat::SetCol (const Standard_Integer Col,
	42	const gp_XYZ& Value) {
	43
	44	Standard_OutOfRange_Raise_if (Col < 1 \|\| Col > 3, " ");
	45	const Standard_Address M = (Standard_Address)&(matrix[0][0]);
	46	if (Col == 1) {
	47	M00 = Value.X(); M10 = Value.Y(); M20 = Value.Z();
	48	}
	49	else if (Col == 2) {
	50	M01 = Value.X(); M11 = Value.Y(); M21 = Value.Z();
	51	}
	52	else {
	53	M02 = Value.X(); M12 = Value.Y(); M22 = Value.Z();
	54	}
	55	}
	56
	57	void gp_Mat::SetCols (const gp_XYZ& Col1,
	58	const gp_XYZ& Col2,
	59	const gp_XYZ& Col3)
	60	{
7fd59977	61	Mat00 = Col1.X(); Mat10 = Col1.Y(); Mat20 = Col1.Z();
	62	Mat01 = Col2.X(); Mat11 = Col2.Y(); Mat21 = Col2.Z();
	63	Mat02 = Col3.X(); Mat12 = Col3.Y(); Mat22 = Col3.Z();
	64	}
	65
	66	void gp_Mat::SetCross (const gp_XYZ& Ref)
	67	{
	68	const Standard_Address M = (Standard_Address)&(matrix[0][0]);
	69	Standard_Real X = Ref.X();
	70	Standard_Real Y = Ref.Y();
	71	Standard_Real Z = Ref.Z();
	72	M00 = M11 = M22 = 0.0;
	73	M01 = - Z;
	74	M02 = Y;
	75	M12 = - X;
	76	M10 = Z;
	77	M20 = - Y;
	78	M21 = X;
	79	}
	80
	81	void gp_Mat::SetDot (const gp_XYZ& Ref)
	82	{
	83	const Standard_Address M = (Standard_Address)&(matrix[0][0]);
	84	Standard_Real X = Ref.X();
	85	Standard_Real Y = Ref.Y();
	86	Standard_Real Z = Ref.Z();
	87	M00 = X * X;
	88	M11 = Y * Y;
	89	M22 = Z * Z;
	90	M01 = X * Y;
	91	M02 = X * Z;
	92	M12 = Y * Z;
	93	M10 = M01;
	94	M20 = M02;
	95	M21 = M12;
	96	}
	97
	98	void gp_Mat::SetRotation (const gp_XYZ& Axis,
	99	const Standard_Real Ang)
	100	{
	101	// Rot = I + sin(Ang) * M + (1. - cos(Ang)) * M*M
	102	// avec M . XYZ = Axis ^ XYZ
	103
	104	// const Standard_Address M = (Standard_Address)&(matrix[0][0]);
	105	gp_XYZ V = Axis.Normalized();
	106	SetCross (V);
	107	Multiply (sin(Ang));
	108	gp_Mat Temp;
	109	Temp.SetScale (1.0);
	110	Add (Temp);
	111	Standard_Real A = V.X();
	112	Standard_Real B = V.Y();
	113	Standard_Real C = V.Z();
	114	Temp.SetRow (1, gp_XYZ(- CC - BB, AB, AC ));
	115	Temp.SetRow (2, gp_XYZ( AB, -AA - CC, BC ));
	116	Temp.SetRow (3, gp_XYZ( AC, BC, - AA - BB));
	117	Temp.Multiply (1.0 - cos(Ang));
	118	Add (Temp);
	119	}
	120
	121	void gp_Mat::SetRow (const Standard_Integer Row,
	122	const gp_XYZ& Value)
	123	{
	124	Standard_OutOfRange_Raise_if (Row < 1 \|\| Row > 3, " ");
125	const Standard_Address M = (Standard_Address)&(matrix[0][0]);
126	if (Row == 1) {
127	M00 = Value.X(); M01 = Value.Y(); M02 = Value.Z();
128	}
129	else if (Row == 2) {
130	M10 = Value.X(); M11 = Value.Y(); M12 = Value.Z();
131	}
132	else {
133	M20 = Value.X(); M21 = Value.Y(); M22 = Value.Z();
134	}
135	}
136
137	void gp_Mat::SetRows (const gp_XYZ& Row1,
138	const gp_XYZ& Row2,
139	const gp_XYZ& Row3)
140	{
141	const Standard_Address M = (Standard_Address)&(matrix[0][0]);
142	M00 = Row1.X(); M01 = Row1.Y(); M02 = Row1.Z();
143	M10 = Row2.X(); M11 = Row2.Y(); M12 = Row2.Z();
144	M20 = Row3.X(); M21 = Row3.Y(); M22 = Row3.Z();
145	}
146
147	gp_XYZ gp_Mat::Column (const Standard_Integer Col) const
148	{
149	Standard_OutOfRange_Raise_if (Col < 1 \|\| Col > 3, "");
150	const Standard_Address M = (Standard_Address)&(matrix[0][0]);
151	if (Col == 1) return gp_XYZ (M00,M10,M20);
152	if (Col == 2) return gp_XYZ (M01,M11,M21);
153	return gp_XYZ (M02,M12,M22);
154	}
155
156	gp_XYZ gp_Mat::Diagonal () const
157	{
158	const Standard_Address M = (Standard_Address)&(matrix[0][0]);
159	return gp_XYZ (M00, M11, M22);
160	}
161
162	gp_XYZ gp_Mat::Row (const Standard_Integer Row) const
163	{
164	Standard_OutOfRange_Raise_if (Row < 1 \|\| Row > 3, "");
165	const Standard_Address M = (Standard_Address)&(matrix[0][0]);
166	if (Row == 1) return gp_XYZ (M00,M01,M02);
167	if (Row == 2) return gp_XYZ (M10,M11,M12);
168	return gp_XYZ (M20,M21,M22);
169	}
170
171	void gp_Mat::Invert ()
172	{
173	Standard_Real new_array[3][3] ;
174	const Standard_Address M = (Standard_Address)&( matrix[0][0]);
175	const Standard_Address N = (Standard_Address)&(new_array[0][0]);
176
177	//
178	// calcul de la transposee de la commatrice
179	//
180	N00 = M11 * M22 - M12 * M21 ;
181	N10 = -(M10 * M22 - M20 * M12) ;
182	N20 = M10 * M21 - M20 * M11 ;
183	N01 = - (M01 * M22 - M21 * M02) ;
184	N11 = M00 * M22 - M20 * M02 ;
185	N21 = -(M00 * M21 - M20 * M01) ;
186	N02 = M01 * M12 - M11 * M02 ;
187	N12 = -(M00 * M12 - M10 * M02) ;
188	N22 = M00 * M11 - M01 * M10 ;
189	Standard_Real det = M00 * N00 + M01* N10 + M02 * N20 ;
190	Standard_Real val = det;
191	if (val < 0) val = - val;
192	Standard_ConstructionError_Raise_if
193	(val <= gp::Resolution(),"");
194	det = 1.0e0 / det ;
195	M00 = N00;
196	M10 = N10;
197	M20 = N20;
198	M01 = N01;
199	M11 = N11;
200	M21 = N21;
201	M02 = N02;
202	M12 = N12;
203	M22 = N22;
204	Multiply(det) ;
205	}
206
207	gp_Mat gp_Mat::Inverted () const
208	{
209	gp_Mat NewMat;
210	const Standard_Address M = (Standard_Address)&( matrix[0][0]);
211	const Standard_Address N = (Standard_Address)&(NewMat.matrix[0][0]);
212	//
213	// calcul de la transposee de la commatrice
214	//
215	N00 = M11 * M22 - M12 * M21 ;
216	N10 = -(M10 * M22 - M20 * M12) ;
217	N20 = M10 * M21 - M20 * M11 ;
218	N01 = - (M01 * M22 - M21 * M02) ;
219	N11 = M00 * M22 - M20 * M02 ;
220	N21 = -(M00 * M21 - M20 * M01) ;
221	N02 = M01 * M12 - M11 * M02 ;
222	N12 = -(M00 * M12 - M10 * M02) ;
223	N22 = M00 * M11 - M01 * M10 ;
224	Standard_Real det = M00 * N00 + M01* N10 + M02 * N20 ;
225	Standard_Real val = det;
226	if (val < 0) val = - val;
227	Standard_ConstructionError_Raise_if
228	(val <= gp::Resolution(),"");
229	det = 1.0e0 / det ;
230	NewMat.Multiply(det) ;
231	return NewMat;
232	}
233
234	void gp_Mat::Power (const Standard_Integer N)
235	{
236	if (N == 1) { }
237	else if (N == 0) { SetIdentity() ; }
238	else if (N == -1) { Invert(); }
239	else {
240	if (N < 0) { Invert(); }
241	Standard_Integer Npower = N;
242	if (Npower < 0) Npower = - Npower;
243	Npower--;
244	gp_Mat Temp = *this;
245	while (1) {
246	if (IsOdd(Npower)) Multiply (Temp);
247	if (Npower == 1) break;
248	Temp.Multiply (Temp);
249	Npower>>=1;
250	}
251	}
252	}
253