• Home
  • Line#
  • Scopes#
  • Navigate#
  • Raw
  • Download
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 
17 #include "btPoint2PointConstraint.h"
18 #include "BulletDynamics/Dynamics/btRigidBody.h"
19 #include <new>
20 
21 
22 
23 
24 
btPoint2PointConstraint(btRigidBody & rbA,btRigidBody & rbB,const btVector3 & pivotInA,const btVector3 & pivotInB)25 btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB)
26 :btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
27 m_flags(0),
28 m_useSolveConstraintObsolete(false)
29 {
30 
31 }
32 
33 
btPoint2PointConstraint(btRigidBody & rbA,const btVector3 & pivotInA)34 btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA)
35 :btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
36 m_flags(0),
37 m_useSolveConstraintObsolete(false)
38 {
39 
40 }
41 
buildJacobian()42 void	btPoint2PointConstraint::buildJacobian()
43 {
44 
45 	///we need it for both methods
46 	{
47 		m_appliedImpulse = btScalar(0.);
48 
49 		btVector3	normal(0,0,0);
50 
51 		for (int i=0;i<3;i++)
52 		{
53 			normal[i] = 1;
54 			new (&m_jac[i]) btJacobianEntry(
55 			m_rbA.getCenterOfMassTransform().getBasis().transpose(),
56 			m_rbB.getCenterOfMassTransform().getBasis().transpose(),
57 			m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(),
58 			m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(),
59 			normal,
60 			m_rbA.getInvInertiaDiagLocal(),
61 			m_rbA.getInvMass(),
62 			m_rbB.getInvInertiaDiagLocal(),
63 			m_rbB.getInvMass());
64 		normal[i] = 0;
65 		}
66 	}
67 
68 
69 }
70 
getInfo1(btConstraintInfo1 * info)71 void btPoint2PointConstraint::getInfo1 (btConstraintInfo1* info)
72 {
73 	getInfo1NonVirtual(info);
74 }
75 
getInfo1NonVirtual(btConstraintInfo1 * info)76 void btPoint2PointConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
77 {
78 	if (m_useSolveConstraintObsolete)
79 	{
80 		info->m_numConstraintRows = 0;
81 		info->nub = 0;
82 	} else
83 	{
84 		info->m_numConstraintRows = 3;
85 		info->nub = 3;
86 	}
87 }
88 
89 
90 
91 
getInfo2(btConstraintInfo2 * info)92 void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* info)
93 {
94 	getInfo2NonVirtual(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
95 }
96 
getInfo2NonVirtual(btConstraintInfo2 * info,const btTransform & body0_trans,const btTransform & body1_trans)97 void btPoint2PointConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans)
98 {
99 	btAssert(!m_useSolveConstraintObsolete);
100 
101 	 //retrieve matrices
102 
103 	// anchor points in global coordinates with respect to body PORs.
104 
105     // set jacobian
106     info->m_J1linearAxis[0] = 1;
107 	info->m_J1linearAxis[info->rowskip+1] = 1;
108 	info->m_J1linearAxis[2*info->rowskip+2] = 1;
109 
110 	btVector3 a1 = body0_trans.getBasis()*getPivotInA();
111 	{
112 		btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
113 		btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
114 		btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
115 		btVector3 a1neg = -a1;
116 		a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
117 	}
118 
119 	info->m_J2linearAxis[0] = -1;
120     info->m_J2linearAxis[info->rowskip+1] = -1;
121     info->m_J2linearAxis[2*info->rowskip+2] = -1;
122 
123 	btVector3 a2 = body1_trans.getBasis()*getPivotInB();
124 
125 	{
126 	//	btVector3 a2n = -a2;
127 		btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
128 		btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
129 		btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
130 		a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
131 	}
132 
133 
134 
135     // set right hand side
136 	btScalar currERP = (m_flags & BT_P2P_FLAGS_ERP) ? m_erp : info->erp;
137     btScalar k = info->fps * currERP;
138     int j;
139 	for (j=0; j<3; j++)
140     {
141         info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
142 		//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
143     }
144 	if(m_flags & BT_P2P_FLAGS_CFM)
145 	{
146 		for (j=0; j<3; j++)
147 		{
148 			info->cfm[j*info->rowskip] = m_cfm;
149 		}
150 	}
151 
152 	btScalar impulseClamp = m_setting.m_impulseClamp;//
153 	for (j=0; j<3; j++)
154     {
155 		if (m_setting.m_impulseClamp > 0)
156 		{
157 			info->m_lowerLimit[j*info->rowskip] = -impulseClamp;
158 			info->m_upperLimit[j*info->rowskip] = impulseClamp;
159 		}
160 	}
161 	info->m_damping = m_setting.m_damping;
162 
163 }
164 
165 
166 
updateRHS(btScalar timeStep)167 void	btPoint2PointConstraint::updateRHS(btScalar	timeStep)
168 {
169 	(void)timeStep;
170 
171 }
172 
173 ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
174 ///If no axis is provided, it uses the default axis for this constraint.
setParam(int num,btScalar value,int axis)175 void btPoint2PointConstraint::setParam(int num, btScalar value, int axis)
176 {
177 	if(axis != -1)
178 	{
179 		btAssertConstrParams(0);
180 	}
181 	else
182 	{
183 		switch(num)
184 		{
185 			case BT_CONSTRAINT_ERP :
186 			case BT_CONSTRAINT_STOP_ERP :
187 				m_erp = value;
188 				m_flags |= BT_P2P_FLAGS_ERP;
189 				break;
190 			case BT_CONSTRAINT_CFM :
191 			case BT_CONSTRAINT_STOP_CFM :
192 				m_cfm = value;
193 				m_flags |= BT_P2P_FLAGS_CFM;
194 				break;
195 			default:
196 				btAssertConstrParams(0);
197 		}
198 	}
199 }
200 
201 ///return the local value of parameter
getParam(int num,int axis) const202 btScalar btPoint2PointConstraint::getParam(int num, int axis) const
203 {
204 	btScalar retVal(SIMD_INFINITY);
205 	if(axis != -1)
206 	{
207 		btAssertConstrParams(0);
208 	}
209 	else
210 	{
211 		switch(num)
212 		{
213 			case BT_CONSTRAINT_ERP :
214 			case BT_CONSTRAINT_STOP_ERP :
215 				btAssertConstrParams(m_flags & BT_P2P_FLAGS_ERP);
216 				retVal = m_erp;
217 				break;
218 			case BT_CONSTRAINT_CFM :
219 			case BT_CONSTRAINT_STOP_CFM :
220 				btAssertConstrParams(m_flags & BT_P2P_FLAGS_CFM);
221 				retVal = m_cfm;
222 				break;
223 			default:
224 				btAssertConstrParams(0);
225 		}
226 	}
227 	return retVal;
228 }
229 
230