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

// Copyright (c) 1995-1999 Matra Datavision
// Copyright (c) 1999-2012 OPEN CASCADE SAS
//
// The content of this file is subject to the Open CASCADE Technology Public
// License Version 6.5 (the "License"). You may not use the content of this file
// except in compliance with the License. Please obtain a copy of the License
// at http://www.opencascade.org and read it completely before using this file.
//
// The Initial Developer of the Original Code is Open CASCADE S.A.S., having its
// main offices at: 1, place des Freres Montgolfier, 78280 Guyancourt, France.
//
// The Original Code and all software distributed under the License is
// distributed on an "AS IS" basis, without warranty of any kind, and the
// Initial Developer hereby disclaims all such warranties, including without
// limitation, any warranties of merchantability, fitness for a particular
// purpose or non-infringement. Please see the License for the specific terms
// and conditions governing the rights and limitations under the License.

//  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
b311480e	1	// Copyright (c) 1995-1999 Matra Datavision
	2	// Copyright (c) 1999-2012 OPEN CASCADE SAS
	3	//
	4	// The content of this file is subject to the Open CASCADE Technology Public
	5	// License Version 6.5 (the "License"). You may not use the content of this file
	6	// except in compliance with the License. Please obtain a copy of the License
	7	// at http://www.opencascade.org and read it completely before using this file.
	8	//
	9	// The Initial Developer of the Original Code is Open CASCADE S.A.S., having its
	10	// main offices at: 1, place des Freres Montgolfier, 78280 Guyancourt, France.
	11	//
	12	// The Original Code and all software distributed under the License is
	13	// distributed on an "AS IS" basis, without warranty of any kind, and the
	14	// Initial Developer hereby disclaims all such warranties, including without
	15	// limitation, any warranties of merchantability, fitness for a particular
	16	// purpose or non-infringement. Please see the License for the specific terms
	17	// and conditions governing the rights and limitations under the License.
	18
7fd59977	19	// 10/09/97 : PMN : Correction BUC40192 (pb avec les matrices negatives)
	20
	21	#ifndef DEB
	22	#define No_Standard_OutOfRange
	23	#define No_Standard_ConstructionError
	24	#endif
	25
	26	#include <gp_Mat.ixx>
	27
	28	#define M00 ((Standard_Real*)M)[0]
	29	#define M01 ((Standard_Real*)M)[1]
	30	#define M02 ((Standard_Real*)M)[2]
	31	#define M10 ((Standard_Real*)M)[3]
	32	#define M11 ((Standard_Real*)M)[4]
	33	#define M12 ((Standard_Real*)M)[5]
	34	#define M20 ((Standard_Real*)M)[6]
	35	#define M21 ((Standard_Real*)M)[7]
	36	#define M22 ((Standard_Real*)M)[8]
	37
	38	#define N00 ((Standard_Real*)N)[0]
	39	#define N01 ((Standard_Real*)N)[1]
	40	#define N02 ((Standard_Real*)N)[2]
	41	#define N10 ((Standard_Real*)N)[3]
	42	#define N11 ((Standard_Real*)N)[4]
	43	#define N12 ((Standard_Real*)N)[5]
	44	#define N20 ((Standard_Real*)N)[6]
	45	#define N21 ((Standard_Real*)N)[7]
	46	#define N22 ((Standard_Real*)N)[8]
	47
	48	gp_Mat::gp_Mat (const gp_XYZ& Col1,
	49	const gp_XYZ& Col2,
	50	const gp_XYZ& Col3)
	51	{
	52	const Standard_Address M = (Standard_Address)&(matrix[0][0]);
	53	M00 = Col1.X(); M10 = Col1.Y(); M20 = Col1.Z();
	54	M01 = Col2.X(); M11 = Col2.Y(); M21 = Col2.Z();
	55	M02 = Col3.X(); M12 = Col3.Y(); M22 = Col3.Z();
	56	}
	57
	58	void gp_Mat::SetCol (const Standard_Integer Col,
	59	const gp_XYZ& Value) {
	60
	61	Standard_OutOfRange_Raise_if (Col < 1 \|\| Col > 3, " ");
	62	const Standard_Address M = (Standard_Address)&(matrix[0][0]);
	63	if (Col == 1) {
	64	M00 = Value.X(); M10 = Value.Y(); M20 = Value.Z();
	65	}
	66	else if (Col == 2) {
	67	M01 = Value.X(); M11 = Value.Y(); M21 = Value.Z();
	68	}
	69	else {
	70	M02 = Value.X(); M12 = Value.Y(); M22 = Value.Z();
	71	}
	72	}
	73
	74	void gp_Mat::SetCols (const gp_XYZ& Col1,
	75	const gp_XYZ& Col2,
	76	const gp_XYZ& Col3)
	77	{
7fd59977	78	Mat00 = Col1.X(); Mat10 = Col1.Y(); Mat20 = Col1.Z();
	79	Mat01 = Col2.X(); Mat11 = Col2.Y(); Mat21 = Col2.Z();
	80	Mat02 = Col3.X(); Mat12 = Col3.Y(); Mat22 = Col3.Z();
	81	}
	82
	83	void gp_Mat::SetCross (const gp_XYZ& Ref)
	84	{
	85	const Standard_Address M = (Standard_Address)&(matrix[0][0]);
	86	Standard_Real X = Ref.X();
	87	Standard_Real Y = Ref.Y();
	88	Standard_Real Z = Ref.Z();
	89	M00 = M11 = M22 = 0.0;
	90	M01 = - Z;
	91	M02 = Y;
	92	M12 = - X;
	93	M10 = Z;
	94	M20 = - Y;
	95	M21 = X;
	96	}
	97
	98	void gp_Mat::SetDot (const gp_XYZ& Ref)
	99	{
	100	const Standard_Address M = (Standard_Address)&(matrix[0][0]);
	101	Standard_Real X = Ref.X();
	102	Standard_Real Y = Ref.Y();
	103	Standard_Real Z = Ref.Z();
	104	M00 = X * X;
	105	M11 = Y * Y;
	106	M22 = Z * Z;
	107	M01 = X * Y;
	108	M02 = X * Z;
	109	M12 = Y * Z;
	110	M10 = M01;
	111	M20 = M02;
	112	M21 = M12;
	113	}
	114
	115	void gp_Mat::SetRotation (const gp_XYZ& Axis,
	116	const Standard_Real Ang)
	117	{
	118	// Rot = I + sin(Ang) * M + (1. - cos(Ang)) * M*M
	119	// avec M . XYZ = Axis ^ XYZ
	120
	121	// const Standard_Address M = (Standard_Address)&(matrix[0][0]);
	122	gp_XYZ V = Axis.Normalized();
	123	SetCross (V);
	124	Multiply (sin(Ang));
	125	gp_Mat Temp;
	126	Temp.SetScale (1.0);
	127	Add (Temp);
	128	Standard_Real A = V.X();
	129	Standard_Real B = V.Y();
	130	Standard_Real C = V.Z();
	131	Temp.SetRow (1, gp_XYZ(- CC - BB, AB, AC ));
	132	Temp.SetRow (2, gp_XYZ( AB, -AA - CC, BC ));
	133	Temp.SetRow (3, gp_XYZ( AC, BC, - AA - BB));
	134	Temp.Multiply (1.0 - cos(Ang));
	135	Add (Temp);
	136	}
	137
	138	void gp_Mat::SetRow (const Standard_Integer Row,
	139	const gp_XYZ& Value)
	140	{
	141	Standard_OutOfRange_Raise_if (Row < 1 \|\| Row > 3, " ");
142	const Standard_Address M = (Standard_Address)&(matrix[0][0]);
143	if (Row == 1) {
144	M00 = Value.X(); M01 = Value.Y(); M02 = Value.Z();
145	}
146	else if (Row == 2) {
147	M10 = Value.X(); M11 = Value.Y(); M12 = Value.Z();
148	}
149	else {
150	M20 = Value.X(); M21 = Value.Y(); M22 = Value.Z();
151	}
152	}
153
154	void gp_Mat::SetRows (const gp_XYZ& Row1,
155	const gp_XYZ& Row2,
156	const gp_XYZ& Row3)
157	{
158	const Standard_Address M = (Standard_Address)&(matrix[0][0]);
159	M00 = Row1.X(); M01 = Row1.Y(); M02 = Row1.Z();
160	M10 = Row2.X(); M11 = Row2.Y(); M12 = Row2.Z();
161	M20 = Row3.X(); M21 = Row3.Y(); M22 = Row3.Z();
162	}
163
164	gp_XYZ gp_Mat::Column (const Standard_Integer Col) const
165	{
166	Standard_OutOfRange_Raise_if (Col < 1 \|\| Col > 3, "");
167	const Standard_Address M = (Standard_Address)&(matrix[0][0]);
168	if (Col == 1) return gp_XYZ (M00,M10,M20);
169	if (Col == 2) return gp_XYZ (M01,M11,M21);
170	return gp_XYZ (M02,M12,M22);
171	}
172
173	gp_XYZ gp_Mat::Diagonal () const
174	{
175	const Standard_Address M = (Standard_Address)&(matrix[0][0]);
176	return gp_XYZ (M00, M11, M22);
177	}
178
179	gp_XYZ gp_Mat::Row (const Standard_Integer Row) const
180	{
181	Standard_OutOfRange_Raise_if (Row < 1 \|\| Row > 3, "");
182	const Standard_Address M = (Standard_Address)&(matrix[0][0]);
183	if (Row == 1) return gp_XYZ (M00,M01,M02);
184	if (Row == 2) return gp_XYZ (M10,M11,M12);
185	return gp_XYZ (M20,M21,M22);
186	}
187
188	void gp_Mat::Invert ()
189	{
190	Standard_Real new_array[3][3] ;
191	const Standard_Address M = (Standard_Address)&( matrix[0][0]);
192	const Standard_Address N = (Standard_Address)&(new_array[0][0]);
193
194	//
195	// calcul de la transposee de la commatrice
196	//
197	N00 = M11 * M22 - M12 * M21 ;
198	N10 = -(M10 * M22 - M20 * M12) ;
199	N20 = M10 * M21 - M20 * M11 ;
200	N01 = - (M01 * M22 - M21 * M02) ;
201	N11 = M00 * M22 - M20 * M02 ;
202	N21 = -(M00 * M21 - M20 * M01) ;
203	N02 = M01 * M12 - M11 * M02 ;
204	N12 = -(M00 * M12 - M10 * M02) ;
205	N22 = M00 * M11 - M01 * M10 ;
206	Standard_Real det = M00 * N00 + M01* N10 + M02 * N20 ;
207	Standard_Real val = det;
208	if (val < 0) val = - val;
209	Standard_ConstructionError_Raise_if
210	(val <= gp::Resolution(),"");
211	det = 1.0e0 / det ;
212	M00 = N00;
213	M10 = N10;
214	M20 = N20;
215	M01 = N01;
216	M11 = N11;
217	M21 = N21;
218	M02 = N02;
219	M12 = N12;
220	M22 = N22;
221	Multiply(det) ;
222	}
223
224	gp_Mat gp_Mat::Inverted () const
225	{
226	gp_Mat NewMat;
227	const Standard_Address M = (Standard_Address)&( matrix[0][0]);
228	const Standard_Address N = (Standard_Address)&(NewMat.matrix[0][0]);
229	//
230	// calcul de la transposee de la commatrice
231	//
232	N00 = M11 * M22 - M12 * M21 ;
233	N10 = -(M10 * M22 - M20 * M12) ;
234	N20 = M10 * M21 - M20 * M11 ;
235	N01 = - (M01 * M22 - M21 * M02) ;
236	N11 = M00 * M22 - M20 * M02 ;
237	N21 = -(M00 * M21 - M20 * M01) ;
238	N02 = M01 * M12 - M11 * M02 ;
239	N12 = -(M00 * M12 - M10 * M02) ;
240	N22 = M00 * M11 - M01 * M10 ;
241	Standard_Real det = M00 * N00 + M01* N10 + M02 * N20 ;
242	Standard_Real val = det;
243	if (val < 0) val = - val;
244	Standard_ConstructionError_Raise_if
245	(val <= gp::Resolution(),"");
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;
262	while (1) {
263	if (IsOdd(Npower)) Multiply (Temp);
264	if (Npower == 1) break;
265	Temp.Multiply (Temp);
266	Npower>>=1;
267	}
268	}
269	}
270