1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15
16 #include "b3Point2PointConstraint.h"
17 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
18
19 #include <new>
20
b3Point2PointConstraint(int rbA,int rbB,const b3Vector3 & pivotInA,const b3Vector3 & pivotInB)21 b3Point2PointConstraint::b3Point2PointConstraint(int rbA, int rbB, const b3Vector3& pivotInA, const b3Vector3& pivotInB)
22 : b3TypedConstraint(B3_POINT2POINT_CONSTRAINT_TYPE, rbA, rbB), m_pivotInA(pivotInA), m_pivotInB(pivotInB), m_flags(0)
23 {
24 }
25
26 /*
27 b3Point2PointConstraint::b3Point2PointConstraint(int rbA,const b3Vector3& pivotInA)
28 :b3TypedConstraint(B3_POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
29 m_flags(0),
30 m_useSolveConstraintObsolete(false)
31 {
32
33 }
34 */
35
getInfo1(b3ConstraintInfo1 * info,const b3RigidBodyData * bodies)36 void b3Point2PointConstraint::getInfo1(b3ConstraintInfo1* info, const b3RigidBodyData* bodies)
37 {
38 getInfo1NonVirtual(info, bodies);
39 }
40
getInfo1NonVirtual(b3ConstraintInfo1 * info,const b3RigidBodyData * bodies)41 void b3Point2PointConstraint::getInfo1NonVirtual(b3ConstraintInfo1* info, const b3RigidBodyData* bodies)
42 {
43 info->m_numConstraintRows = 3;
44 info->nub = 3;
45 }
46
getInfo2(b3ConstraintInfo2 * info,const b3RigidBodyData * bodies)47 void b3Point2PointConstraint::getInfo2(b3ConstraintInfo2* info, const b3RigidBodyData* bodies)
48 {
49 b3Transform trA;
50 trA.setIdentity();
51 trA.setOrigin(bodies[m_rbA].m_pos);
52 trA.setRotation(bodies[m_rbA].m_quat);
53
54 b3Transform trB;
55 trB.setIdentity();
56 trB.setOrigin(bodies[m_rbB].m_pos);
57 trB.setRotation(bodies[m_rbB].m_quat);
58
59 getInfo2NonVirtual(info, trA, trB);
60 }
61
getInfo2NonVirtual(b3ConstraintInfo2 * info,const b3Transform & body0_trans,const b3Transform & body1_trans)62 void b3Point2PointConstraint::getInfo2NonVirtual(b3ConstraintInfo2* info, const b3Transform& body0_trans, const b3Transform& body1_trans)
63 {
64 //retrieve matrices
65
66 // anchor points in global coordinates with respect to body PORs.
67
68 // set jacobian
69 info->m_J1linearAxis[0] = 1;
70 info->m_J1linearAxis[info->rowskip + 1] = 1;
71 info->m_J1linearAxis[2 * info->rowskip + 2] = 1;
72
73 b3Vector3 a1 = body0_trans.getBasis() * getPivotInA();
74 //b3Vector3 a1a = b3QuatRotate(body0_trans.getRotation(),getPivotInA());
75
76 {
77 b3Vector3* angular0 = (b3Vector3*)(info->m_J1angularAxis);
78 b3Vector3* angular1 = (b3Vector3*)(info->m_J1angularAxis + info->rowskip);
79 b3Vector3* angular2 = (b3Vector3*)(info->m_J1angularAxis + 2 * info->rowskip);
80 b3Vector3 a1neg = -a1;
81 a1neg.getSkewSymmetricMatrix(angular0, angular1, angular2);
82 }
83
84 if (info->m_J2linearAxis)
85 {
86 info->m_J2linearAxis[0] = -1;
87 info->m_J2linearAxis[info->rowskip + 1] = -1;
88 info->m_J2linearAxis[2 * info->rowskip + 2] = -1;
89 }
90
91 b3Vector3 a2 = body1_trans.getBasis() * getPivotInB();
92
93 {
94 // b3Vector3 a2n = -a2;
95 b3Vector3* angular0 = (b3Vector3*)(info->m_J2angularAxis);
96 b3Vector3* angular1 = (b3Vector3*)(info->m_J2angularAxis + info->rowskip);
97 b3Vector3* angular2 = (b3Vector3*)(info->m_J2angularAxis + 2 * info->rowskip);
98 a2.getSkewSymmetricMatrix(angular0, angular1, angular2);
99 }
100
101 // set right hand side
102 b3Scalar currERP = (m_flags & B3_P2P_FLAGS_ERP) ? m_erp : info->erp;
103 b3Scalar k = info->fps * currERP;
104 int j;
105 for (j = 0; j < 3; j++)
106 {
107 info->m_constraintError[j * info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
108 //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
109 }
110 if (m_flags & B3_P2P_FLAGS_CFM)
111 {
112 for (j = 0; j < 3; j++)
113 {
114 info->cfm[j * info->rowskip] = m_cfm;
115 }
116 }
117
118 b3Scalar impulseClamp = m_setting.m_impulseClamp; //
119 for (j = 0; j < 3; j++)
120 {
121 if (m_setting.m_impulseClamp > 0)
122 {
123 info->m_lowerLimit[j * info->rowskip] = -impulseClamp;
124 info->m_upperLimit[j * info->rowskip] = impulseClamp;
125 }
126 }
127 info->m_damping = m_setting.m_damping;
128 }
129
updateRHS(b3Scalar timeStep)130 void b3Point2PointConstraint::updateRHS(b3Scalar timeStep)
131 {
132 (void)timeStep;
133 }
134
135 ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
136 ///If no axis is provided, it uses the default axis for this constraint.
setParam(int num,b3Scalar value,int axis)137 void b3Point2PointConstraint::setParam(int num, b3Scalar value, int axis)
138 {
139 if (axis != -1)
140 {
141 b3AssertConstrParams(0);
142 }
143 else
144 {
145 switch (num)
146 {
147 case B3_CONSTRAINT_ERP:
148 case B3_CONSTRAINT_STOP_ERP:
149 m_erp = value;
150 m_flags |= B3_P2P_FLAGS_ERP;
151 break;
152 case B3_CONSTRAINT_CFM:
153 case B3_CONSTRAINT_STOP_CFM:
154 m_cfm = value;
155 m_flags |= B3_P2P_FLAGS_CFM;
156 break;
157 default:
158 b3AssertConstrParams(0);
159 }
160 }
161 }
162
163 ///return the local value of parameter
getParam(int num,int axis) const164 b3Scalar b3Point2PointConstraint::getParam(int num, int axis) const
165 {
166 b3Scalar retVal(B3_INFINITY);
167 if (axis != -1)
168 {
169 b3AssertConstrParams(0);
170 }
171 else
172 {
173 switch (num)
174 {
175 case B3_CONSTRAINT_ERP:
176 case B3_CONSTRAINT_STOP_ERP:
177 b3AssertConstrParams(m_flags & B3_P2P_FLAGS_ERP);
178 retVal = m_erp;
179 break;
180 case B3_CONSTRAINT_CFM:
181 case B3_CONSTRAINT_STOP_CFM:
182 b3AssertConstrParams(m_flags & B3_P2P_FLAGS_CFM);
183 retVal = m_cfm;
184 break;
185 default:
186 b3AssertConstrParams(0);
187 }
188 }
189 return retVal;
190 }
191