Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/SuperOrxoBros_HS18/SuperOrxoBros_HS18/src/external/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.cpp @ 12175

Last change on this file since 12175 was 12175, checked in by siramesh, 5 years ago

Super Orxo Bros (Sidharth Ramesh, Nisa Balta, Jeff Ren)

File size: 5.7 KB
Line 
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. This notice may not be removed or altered from any source distribution.
14*/
15
16
17#include "btContactConstraint.h"
18#include "BulletDynamics/Dynamics/btRigidBody.h"
19#include "LinearMath/btVector3.h"
20#include "btJacobianEntry.h"
21#include "btContactSolverInfo.h"
22#include "LinearMath/btMinMax.h"
23#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
24
25
26
27btContactConstraint::btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB)
28:btTypedConstraint(CONTACT_CONSTRAINT_TYPE,rbA,rbB),
29        m_contactManifold(*contactManifold)
30{
31
32}
33
34btContactConstraint::~btContactConstraint()
35{
36
37}
38
39void    btContactConstraint::setContactManifold(btPersistentManifold* contactManifold)
40{
41        m_contactManifold = *contactManifold;
42}
43
44void btContactConstraint::getInfo1 (btConstraintInfo1* info)
45{
46
47}
48
49void btContactConstraint::getInfo2 (btConstraintInfo2* info)
50{
51
52}
53
54void    btContactConstraint::buildJacobian()
55{
56
57}
58
59
60
61
62
63#include "btContactConstraint.h"
64#include "BulletDynamics/Dynamics/btRigidBody.h"
65#include "LinearMath/btVector3.h"
66#include "btJacobianEntry.h"
67#include "btContactSolverInfo.h"
68#include "LinearMath/btMinMax.h"
69#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
70
71
72
73//response  between two dynamic objects without friction, assuming 0 penetration depth
74btScalar resolveSingleCollision(
75        btRigidBody* body1,
76        btCollisionObject* colObj2,
77                const btVector3& contactPositionWorld,
78                const btVector3& contactNormalOnB,
79        const btContactSolverInfo& solverInfo,
80                btScalar distance)
81{
82        btRigidBody* body2 = btRigidBody::upcast(colObj2);
83   
84       
85    const btVector3& normal = contactNormalOnB;
86
87    btVector3 rel_pos1 = contactPositionWorld - body1->getWorldTransform().getOrigin(); 
88    btVector3 rel_pos2 = contactPositionWorld - colObj2->getWorldTransform().getOrigin();
89   
90    btVector3 vel1 = body1->getVelocityInLocalPoint(rel_pos1);
91        btVector3 vel2 = body2? body2->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
92    btVector3 vel = vel1 - vel2;
93    btScalar rel_vel;
94    rel_vel = normal.dot(vel);
95   
96    btScalar combinedRestitution = body1->getRestitution() * colObj2->getRestitution();
97    btScalar restitution = combinedRestitution* -rel_vel;
98
99    btScalar positionalError = solverInfo.m_erp *-distance /solverInfo.m_timeStep ;
100    btScalar velocityError = -(1.0f + restitution) * rel_vel;// * damping;
101        btScalar denom0 = body1->computeImpulseDenominator(contactPositionWorld,normal);
102        btScalar denom1 = body2? body2->computeImpulseDenominator(contactPositionWorld,normal) : 0.f;
103        btScalar relaxation = 1.f;
104        btScalar jacDiagABInv = relaxation/(denom0+denom1);
105
106    btScalar penetrationImpulse = positionalError * jacDiagABInv;
107    btScalar velocityImpulse = velocityError * jacDiagABInv;
108
109    btScalar normalImpulse = penetrationImpulse+velocityImpulse;
110    normalImpulse = 0.f > normalImpulse ? 0.f: normalImpulse;
111
112        body1->applyImpulse(normal*(normalImpulse), rel_pos1);
113    if (body2)
114                body2->applyImpulse(-normal*(normalImpulse), rel_pos2);
115   
116    return normalImpulse;
117}
118
119
120//bilateral constraint between two dynamic objects
121void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
122                      btRigidBody& body2, const btVector3& pos2,
123                      btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep)
124{
125        (void)timeStep;
126        (void)distance;
127
128
129        btScalar normalLenSqr = normal.length2();
130        btAssert(btFabs(normalLenSqr) < btScalar(1.1));
131        if (normalLenSqr > btScalar(1.1))
132        {
133                impulse = btScalar(0.);
134                return;
135        }
136        btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
137        btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
138        //this jacobian entry could be re-used for all iterations
139       
140        btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
141        btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
142        btVector3 vel = vel1 - vel2;
143       
144
145           btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
146                body2.getCenterOfMassTransform().getBasis().transpose(),
147                rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(),
148                body2.getInvInertiaDiagLocal(),body2.getInvMass());
149
150        btScalar jacDiagAB = jac.getDiagonal();
151        btScalar jacDiagABInv = btScalar(1.) / jacDiagAB;
152       
153          btScalar rel_vel = jac.getRelativeVelocity(
154                body1.getLinearVelocity(),
155                body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(),
156                body2.getLinearVelocity(),
157                body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity()); 
158        btScalar a;
159        a=jacDiagABInv;
160
161
162        rel_vel = normal.dot(vel);
163       
164        //todo: move this into proper structure
165        btScalar contactDamping = btScalar(0.2);
166
167#ifdef ONLY_USE_LINEAR_MASS
168        btScalar massTerm = btScalar(1.) / (body1.getInvMass() + body2.getInvMass());
169        impulse = - contactDamping * rel_vel * massTerm;
170#else   
171        btScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
172        impulse = velocityImpulse;
173#endif
174}
175
176
177
178
Note: See TracBrowser for help on using the repository browser.