Newton Dynamics  4.00
dSpatialMatrix.h
1 /* Copyright (c) <2003-2019> <Julio Jerez, Newton Game Dynamics>
2 *
3 * This software is provided 'as-is', without any express or implied
4 * warranty. In no event will the authors be held liable for any damages
5 * arising from the use of this software.
6 *
7 * Permission is granted to anyone to use this software for any purpose,
8 * including commercial applications, and to alter it and redistribute it
9 * freely, subject to the following restrictions:
10 *
11 * 1. The origin of this software must not be misrepresented; you must not
12 * claim that you wrote the original software. If you use this software
13 * in a product, an acknowledgment in the product documentation would be
14 * appreciated but is not required.
15 *
16 * 2. Altered source versions must be plainly marked as such, and must not be
17 * misrepresented as being the original software.
18 *
19 * 3. This notice may not be removed or altered from any source distribution.
20 */
21 
22 #ifndef __D_SPATIAL_MATRIX_H__
23 #define __D_SPATIAL_MATRIX_H__
24 
25 #include "dCoreStdafx.h"
26 #include "dTypes.h"
27 #include "dVector.h"
28 
30 {
31  public:
32  D_INLINE dSpatialMatrix()
33  {
34  }
35 
36  D_INLINE dSpatialMatrix(dFloat32 val)
37  {
38  const dSpatialVector row (val);
39  for (dInt32 i = 0; i < 6; i++)
40  {
41  m_rows[i] = row;
42  }
43  }
44 
45  D_INLINE dSpatialVector& operator[] (dInt32 i)
46  {
47  dAssert(i < 6);
48  dAssert(i >= 0);
49  return m_rows[i];
50  }
51 
52  D_INLINE const dSpatialVector& operator[] (dInt32 i) const
53  {
54  dAssert(i < 6);
55  dAssert(i >= 0);
56  return m_rows[i];
57  }
58 
59  D_CORE_API dSpatialMatrix Inverse(dInt32 rows) const;
60 
61  D_INLINE dSpatialVector VectorTimeMatrix(const dSpatialVector& jacobian) const
62  {
63  dSpatialVector tmp(m_rows[0].Scale (jacobian[0]));
64  for (dInt32 i = 1; i < 6; i++)
65  {
66  tmp = tmp + m_rows[i].Scale(jacobian[i]);
67  }
68  return tmp;
69  }
70 
71  D_INLINE dSpatialVector VectorTimeMatrix(const dSpatialVector& jacobian, dInt32 dof) const
72  {
73  dSpatialVector tmp(dFloat32 (0.0f));
74  for (dInt32 i = 0; i < dof; i++)
75  {
76  tmp = tmp + m_rows[i].Scale(jacobian[i]);
77  }
78  return tmp;
79  }
80 
81  dSpatialVector m_rows[6];
82 };
83 
84 #endif
85 
dSpatialMatrix
Definition: dSpatialMatrix.h:30
dSpatialVector
Definition: dVectorArmNeon.h:1954