mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-16 06:30:05 +00:00
Minor constraint refactoring, to allow SPU-side processing for PLAYSTATION 3 (added non-virtual methods)
Also comment-out some code for __SPU__ to reduce code size Added btContactConstraint (only used on PS3 SPU right now, better to use btPersistentManifold directly for contact constraints) Improved readblend utility library (see also usage in http://gamekit.googlecode.com with Irrlicht) Fix for btConvexConvexAlgorithm, potential division by zero Thanks linzner http://code.google.com/p/bullet/issues/detail?id=260
This commit is contained in:
erwin.coumans
parent
b16f251530
commit
3e2529fcb5
@ -10,13 +10,18 @@
|
||||
#ifndef _ABS_FILE_H
|
||||
#define _ABS_FILE_H
|
||||
|
||||
typedef unsigned char uint8_t;
|
||||
typedef unsigned long int uint64_t;
|
||||
typedef unsigned int uint32_t;
|
||||
typedef int int32_t;
|
||||
#ifdef WIN32
|
||||
|
||||
typedef unsigned char uint8_t;
|
||||
typedef unsigned long int uint64_t;
|
||||
typedef unsigned int uint32_t;
|
||||
typedef int int32_t;
|
||||
typedef unsigned short uint16_t;
|
||||
typedef short int16_t;
|
||||
#else
|
||||
#include <stdint.h>
|
||||
|
||||
#endif
|
||||
|
||||
/*
|
||||
PLEASE NOTE: This license applies to abs-file.h ONLY; The version of
|
||||
@ -194,6 +199,8 @@ static int MY_PUTS(const char *s, MY_FILETYPE *const fp) {
|
||||
|
||||
#else /* !USE_PHYSFS */
|
||||
|
||||
//#define USE_POSIX_FILES 1
|
||||
#ifdef USE_POSIX_FILES
|
||||
#define MY_FILETYPE FILE
|
||||
#define MY_READ(p,s,n,fp) fread(p,s,n,fp)
|
||||
#define MY_WRITE(p,s,n,fp) fwrite(p,s,n,fp)
|
||||
@ -221,6 +228,33 @@ static long MY_FILELENGTH(FILE *fp) {
|
||||
fseek(fp, currentpos, SEEK_SET); /* restore previous cursor position */
|
||||
return newpos;
|
||||
}
|
||||
#else
|
||||
#include <string.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#define MY_FILETYPE char
|
||||
|
||||
|
||||
long MY_FILELENGTH(FILE *fp);
|
||||
MY_FILETYPE* MY_OPEN_FOR_READ(const char *const filename);
|
||||
int MY_GETC(MY_FILETYPE *const fp);
|
||||
void MY_SEEK(MY_FILETYPE* fp,int pos);
|
||||
#define MY_REWIND(fp) MY_SEEK(fp,0)
|
||||
int MY_READ(unsigned char* dest,int size,int num,MY_FILETYPE* fp);
|
||||
|
||||
void MY_CLOSE(MY_FILETYPE* fp);
|
||||
int MY_TELL(MY_FILETYPE* fp);
|
||||
int MY_ATEOF(MY_FILETYPE* fp);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif //USE_POSIX_FILES
|
||||
|
||||
#endif /* USE_PHYSFS */
|
||||
|
||||
#endif /* _ABS_FILE_H */
|
||||
|
||||
87
Extras/readblend/blendtype.h
Normal file
87
Extras/readblend/blendtype.h
Normal file
@ -0,0 +1,87 @@
|
||||
#ifndef BLEND_TYPE_H
|
||||
#define BLEND_TYPE_H
|
||||
#include "abs-file.h"
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
typedef struct _BlendField BlendField;
|
||||
struct _BlendField {
|
||||
char* field_bytes;
|
||||
int field_bytes_count;
|
||||
|
||||
/* the offset into field_bytes at which each field of a
|
||||
structure begins. this is so we can keep the data aligned
|
||||
correctly for various architectures. a non-structure type
|
||||
is equivalent to a structure with a single field.
|
||||
*/
|
||||
int* field_offsets;
|
||||
int field_offsets_count;
|
||||
};
|
||||
struct _BlendBlock {
|
||||
char tag[5];
|
||||
uint32_t blender_pointer;
|
||||
/*void* fixed_pointer;*/
|
||||
|
||||
int type_index;
|
||||
/* a block is simply an array of its type as defined by type_index.
|
||||
array_entries is an array of pointers to each entry in the block's
|
||||
array.
|
||||
*/
|
||||
BlendField *array_entries;
|
||||
int array_entries_count;
|
||||
void* customData; /* for Link blocks, with custom data such as .jpg pictures etc */
|
||||
int customDataSize;
|
||||
};
|
||||
|
||||
|
||||
typedef struct _BlendBlock BlendBlock;
|
||||
|
||||
/* the types extracted from the Blender file */
|
||||
typedef struct _BlendType BlendType;
|
||||
struct _BlendType {
|
||||
char* name;
|
||||
int size;
|
||||
|
||||
int is_struct;
|
||||
|
||||
/* if is_struct... this defines the types of each of the structure fields */
|
||||
int fieldtypes_count;
|
||||
int* fieldtypes; /* type indices */
|
||||
int fieldnames_count;
|
||||
int* fieldnames; /* name indices */
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/* the opaque BlendFile structure */
|
||||
typedef struct _BlendFile{
|
||||
BlendType* types;
|
||||
int types_count;
|
||||
|
||||
char* *names;
|
||||
int names_count;
|
||||
|
||||
int* strc_indices;
|
||||
int strc_indices_count;
|
||||
|
||||
BlendBlock *blocks;
|
||||
int blocks_count;
|
||||
|
||||
int name_undef; /* index of the name we add specially for top blocks */
|
||||
|
||||
} BlendFile;
|
||||
|
||||
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif //BLEND_TYPE_H
|
||||
File diff suppressed because it is too large
Load Diff
@ -38,11 +38,11 @@
|
||||
/* TODO: Doxygen me. */
|
||||
|
||||
|
||||
/* don't worry yourself about this. */
|
||||
#ifndef BlendFile
|
||||
#define BlendFile void
|
||||
#endif
|
||||
#include "blendtype.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
@ -101,6 +101,7 @@ typedef struct {
|
||||
long entry_index; long field_index;
|
||||
} BlendObject;
|
||||
|
||||
|
||||
/* The callback type for passing to blend_foreach_block() (the callback
|
||||
should return zero if it doesn't want to see any more blocks.) */
|
||||
#define BLENDBLOCKCALLBACK_RETURN int
|
||||
@ -312,20 +313,41 @@ typedef struct {
|
||||
bDeformWeight* deform_weights;
|
||||
int deform_weights_count;
|
||||
} bVert;
|
||||
|
||||
|
||||
#define BVERT_HAS_CNORMAL(BV) ((BV)->cnormal[0] != 0.0f || \
|
||||
(BV)->cnormal[1] != 0.0f || \
|
||||
(BV)->cnormal[2] != 0.0f)
|
||||
|
||||
#define BFACE_FLAG_SMOOTH 0x01
|
||||
|
||||
typedef struct _bImage {
|
||||
|
||||
void* m_packedImagePtr;
|
||||
int m_sizePackedImage;
|
||||
char m_imagePathName [128];
|
||||
|
||||
int m_ok;
|
||||
|
||||
int m_xrep;
|
||||
int m_yrep;
|
||||
|
||||
} bImage;
|
||||
|
||||
typedef struct {
|
||||
int v[4];
|
||||
float rgba[4][4]; /* vertex colours */
|
||||
float rgba2[4][4]; /* texture face colours (errrr...?) */
|
||||
float uv[4][2];
|
||||
BlendBlockPointer image_id;
|
||||
bImage* m_image;
|
||||
int mat;
|
||||
unsigned char flags;
|
||||
int m_flag;
|
||||
int m_mode;
|
||||
BlendBlockPointer image_id;
|
||||
} bFace;
|
||||
|
||||
|
||||
#define BFACE_HAS_TEXTURE(BF) ((BF)->image_id != NULL)
|
||||
#define BFACE_IS_QUAD(BF) ((BF)->v[3] != 0)
|
||||
#define BFACE_IS_TRI(BF) ((BF)->v[2] != 0 && (BF)->v[3] == 0)
|
||||
@ -389,7 +411,7 @@ typedef struct {
|
||||
|
||||
typedef float bMatrix[4][4];
|
||||
|
||||
typedef struct {
|
||||
typedef struct _bMesh{
|
||||
bVert *vert;
|
||||
int vert_count;
|
||||
bFace *face;
|
||||
@ -400,14 +422,17 @@ typedef struct {
|
||||
typedef enum {
|
||||
BOBJ_TYPE_UNKNOWN,
|
||||
BOBJ_TYPE_NULL, /* indicates object has no data associated with it! */
|
||||
BOBJ_TYPE_MESH
|
||||
BOBJ_TYPE_MESH,
|
||||
BOBJ_TYPE_INVALID_MESH,
|
||||
BOBJ_TYPE_CAMERA,
|
||||
BOBJ_TYPE_LAMP
|
||||
} bObjType;
|
||||
|
||||
typedef enum {
|
||||
BAQ_INCLUDE_CHILDREN = 0x0001
|
||||
} bAcquireFlags;
|
||||
|
||||
typedef struct {
|
||||
typedef struct _bObj{
|
||||
bObjType type;
|
||||
char *name;
|
||||
|
||||
@ -417,11 +442,15 @@ typedef struct {
|
||||
float rotphr[3]; /* pitch/head/roll rotation component of transform (use for normals) */
|
||||
float location[3]; /* location component of transform */
|
||||
unsigned char transflags; /* NOT DECODED YET, RAW BYTE */
|
||||
|
||||
float mass;//used for rigid body dynamics
|
||||
int gameflag; //used to detect object type (static, ghost, dynamic, rigid body, soft body)
|
||||
int boundtype; //used to detect collision shape type
|
||||
|
||||
union {
|
||||
void *dummy;
|
||||
bMesh *mesh;
|
||||
} data;
|
||||
|
||||
} bObj;
|
||||
|
||||
|
||||
@ -456,4 +485,8 @@ void blend_free_texlayer(bTexLayer *tl);
|
||||
void blend_acquire_texlayer_from_obj(BlendFile *bf, BlendObject *tlobj,
|
||||
bTexLayer *tl);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
@ -4,107 +4,170 @@
|
||||
#include "abs-file.h"
|
||||
|
||||
#include "readblend.h"
|
||||
#include "blendtype.h"
|
||||
|
||||
#define MAX_MESHES 10
|
||||
bMesh gMeshes[MAX_MESHES];
|
||||
int gNumMesh = 0;
|
||||
|
||||
#define MAX_OBJECTS 10
|
||||
bObj gObjects[MAX_OBJECTS];
|
||||
int gNumObjects = 0;
|
||||
|
||||
void crawl(BlendFile* blend_file,
|
||||
BlendObject obj)
|
||||
BlendObject obj)
|
||||
{
|
||||
BlendObject data_obj;
|
||||
BlendObject data_obj;
|
||||
BlendObject data_obj2;
|
||||
|
||||
BlendBlock* tmpBlock=0;
|
||||
|
||||
if (blend_object_structure_getfield(blend_file, &data_obj, obj, "totvert")) {
|
||||
if (blend_object_type(blend_file, data_obj) == BLEND_OBJ_LONG32) {
|
||||
long data1 = 123456;
|
||||
if (blend_object_getdata(blend_file, &data1, data_obj)) {
|
||||
fprintf(stderr, "TOTVERT=(%ld) ", data1);
|
||||
} else {
|
||||
fprintf(stderr, "FETCH ERROR\n");
|
||||
}
|
||||
}
|
||||
}
|
||||
{
|
||||
const char* type_name = blend_file->types[obj.type].name;
|
||||
if (strcmp(type_name,"Object")==0)
|
||||
{
|
||||
if (gNumObjects<MAX_OBJECTS)
|
||||
{
|
||||
blend_acquire_obj_from_obj(blend_file,&obj,&gObjects[gNumObjects],0);
|
||||
gNumObjects++;
|
||||
}
|
||||
}
|
||||
|
||||
if (blend_object_structure_getfield(blend_file, &data_obj, obj, "co")) {
|
||||
if (blend_object_type(blend_file, data_obj) == BLEND_OBJ_FLOAT) {
|
||||
float data1 = 123456.7;
|
||||
float data2 = 123456.8;
|
||||
float data3 = 123456.9;
|
||||
if (blend_object_array_getdata(blend_file, &data1, data_obj, 0,0) &&
|
||||
blend_object_array_getdata(blend_file, &data2, data_obj, 0,1) &&
|
||||
blend_object_array_getdata(blend_file, &data3, data_obj, 0,2)) {
|
||||
fprintf(stderr, "CO=(%f,%f,%f) ", data1, data2, data3);
|
||||
} else {
|
||||
fprintf(stderr, "FETCH ERROR\n");
|
||||
}
|
||||
}
|
||||
}
|
||||
if (strcmp(type_name,"Mesh")==0)
|
||||
{
|
||||
printf("object type_name = %s\n",type_name);
|
||||
if (gNumMesh<MAX_MESHES)
|
||||
{
|
||||
blend_acquire_mesh_from_obj(blend_file, &obj, &gMeshes[gNumMesh]);
|
||||
gNumMesh++;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
///todo: compress those 4 lines into a one-liner, using a C++ wrapper
|
||||
if (blend_object_structure_getfield(blend_file, &data_obj, obj, "curscene")) {
|
||||
if (blend_object_type(blend_file, data_obj) == BLEND_OBJ_POINTER) {
|
||||
if (blend_object_getdata(blend_file, &tmpBlock, data_obj)) {
|
||||
BlendObject blscene= blend_block_get_object(blend_file,tmpBlock,0);
|
||||
|
||||
if (blend_object_structure_getfield(blend_file, &data_obj,blscene , "id"))
|
||||
{
|
||||
if (blend_object_type(blend_file, data_obj) == BLEND_OBJ_STRUCT)
|
||||
{
|
||||
if (blend_object_structure_getfield(blend_file, &data_obj2,data_obj , "name"))
|
||||
{
|
||||
char dest[19];
|
||||
int max_chars=20;
|
||||
|
||||
if (blend_object_getstring(blend_file, data_obj2,
|
||||
dest, max_chars))
|
||||
{
|
||||
printf("dest=%s\n",dest);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
if (blend_object_structure_getfield(blend_file, &data_obj,blscene , "world")) {
|
||||
if (blend_object_type(blend_file, data_obj) == BLEND_OBJ_POINTER) {
|
||||
if (blend_object_getdata(blend_file, &tmpBlock, data_obj)) {
|
||||
BlendObject blworld= blend_block_get_object(blend_file,tmpBlock,0);
|
||||
|
||||
if (blend_object_structure_getfield(blend_file, &data_obj, blworld, "gravity"))
|
||||
{
|
||||
printf("Scene with world and gravity\n");
|
||||
if (blend_object_type(blend_file, data_obj) == BLEND_OBJ_FLOAT) {
|
||||
float data1 = 123456.7;
|
||||
if (blend_object_getdata(blend_file, &data1, data_obj))
|
||||
fprintf(stderr, "gravity=(%f) ", data1);
|
||||
} else {
|
||||
fprintf(stderr, "FETCH ERROR\n");
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static BLENDBLOCKCALLBACK_RETURN
|
||||
blockiter(BLENDBLOCKCALLBACK_ARGS) {
|
||||
int i;
|
||||
int entry_count = blend_block_get_entry_count(blend_file, block);
|
||||
int i;
|
||||
int entry_count = blend_block_get_entry_count(blend_file, block);
|
||||
#if 0
|
||||
const char* tagname = blend_block_get_tagname(blend_file, block);
|
||||
const char* typename = blend_block_get_typename(blend_file, block);
|
||||
fprintf(stderr, "block: tag=%s type=%s entries=%d ",
|
||||
tagname, typename, entry_count);
|
||||
const char* tagname = blend_block_get_tagname(blend_file, block);
|
||||
const char* typename = blend_block_get_typename(blend_file, block);
|
||||
fprintf(stderr, "block: tag=%s type=%s entries=%d ",
|
||||
tagname, typename, entry_count);
|
||||
#endif
|
||||
for (i=0; i<entry_count; ++i) {
|
||||
BlendObject obj = blend_block_get_object(blend_file, block, i);
|
||||
crawl(blend_file, obj);
|
||||
}
|
||||
//fprintf(stderr, "\n");
|
||||
return 1;
|
||||
for (i=0; i<entry_count; ++i) {
|
||||
BlendObject obj = blend_block_get_object(blend_file, block, i);
|
||||
crawl(blend_file, obj);
|
||||
}
|
||||
//fprintf(stderr, "\n");
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
||||
int
|
||||
main(int argc, char *argv[])
|
||||
{
|
||||
MY_FILETYPE *file;
|
||||
BlendFile *bf;
|
||||
int rtn = 0;
|
||||
MY_FILETYPE *file;
|
||||
BlendFile *bf;
|
||||
int rtn = 0;
|
||||
|
||||
if (argc <= 1) {
|
||||
fprintf(stderr, "specify filename on command line.\n");
|
||||
return -1;
|
||||
}
|
||||
if (argc <= 1) {
|
||||
fprintf(stderr, "specify filename on command line.\n");
|
||||
return -1;
|
||||
}
|
||||
|
||||
file = MY_OPEN_FOR_READ(argv[1]);
|
||||
file = MY_OPEN_FOR_READ(argv[1]);
|
||||
|
||||
if (!file) {
|
||||
fprintf(stderr, "couldn't open file. :(\n");
|
||||
return -1;
|
||||
}
|
||||
if (!file) {
|
||||
fprintf(stderr, "couldn't open file. :(\n");
|
||||
return -1;
|
||||
}
|
||||
|
||||
bf = blend_read(file);
|
||||
bf = blend_read(file);
|
||||
|
||||
if (!bf) {
|
||||
fprintf(stderr, "couldn't read blender file. :(\n");
|
||||
rtn = -1;
|
||||
goto closeit;
|
||||
}
|
||||
if (!bf) {
|
||||
fprintf(stderr, "couldn't read blender file. :(\n");
|
||||
rtn = -1;
|
||||
goto closeit;
|
||||
}
|
||||
|
||||
blend_dump_typedefs(bf);
|
||||
blend_dump_typedefs(bf);
|
||||
|
||||
blend_dump_blocks(bf);
|
||||
// blend_dump_blocks(bf);
|
||||
|
||||
fflush(stdout);
|
||||
fflush(stdout);
|
||||
|
||||
blend_foreach_block(bf, blockiter, NULL);
|
||||
blend_foreach_block(bf, blockiter, NULL);
|
||||
|
||||
|
||||
{
|
||||
BlendObject obj;
|
||||
char want_name[] = "IMmetalrock.jpg";
|
||||
if (blend_object_get_by_IDname(bf,&obj, want_name)) {
|
||||
fprintf(stderr, "got %s.\n", want_name);
|
||||
}
|
||||
}
|
||||
{
|
||||
BlendObject obj;
|
||||
char want_name[] = "IMmetalrock.jpg";
|
||||
if (blend_object_get_by_IDname(bf,&obj, want_name)) {
|
||||
fprintf(stderr, "got %s.\n", want_name);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
blend_free(bf);
|
||||
blend_free(bf);
|
||||
|
||||
closeit:
|
||||
MY_CLOSE(file);
|
||||
return rtn;
|
||||
closeit:
|
||||
MY_CLOSE(file);
|
||||
return rtn;
|
||||
}
|
||||
|
||||
@ -207,12 +207,24 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
|
||||
input.m_transformB = body1->getWorldTransform();
|
||||
|
||||
gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
|
||||
btScalar sepDist = gjkPairDetector.getCachedSeparatingDistance()+dispatchInfo.m_convexConservativeDistanceThreshold;
|
||||
|
||||
//now perturbe directions to get multiple contact points
|
||||
btVector3 v0,v1;
|
||||
btVector3 sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized();
|
||||
btPlaneSpace1(sepNormalWorldSpace,v0,v1);
|
||||
btVector3 sepNormalWorldSpace;
|
||||
btScalar sepDist = 0.f;
|
||||
|
||||
#ifdef USE_SEPDISTANCE_UTIL2
|
||||
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
|
||||
{
|
||||
sepDist = gjkPairDetector.getCachedSeparatingDistance()+dispatchInfo.m_convexConservativeDistanceThreshold;
|
||||
if (sepDist>SIMD_EPSILON)
|
||||
{
|
||||
//now perturbe directions to get multiple contact points
|
||||
sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized();
|
||||
btPlaneSpace1(sepNormalWorldSpace,v0,v1);
|
||||
}
|
||||
}
|
||||
#endif //USE_SEPDISTANCE_UTIL2
|
||||
|
||||
//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
|
||||
|
||||
//perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
|
||||
@ -280,7 +292,7 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
|
||||
|
||||
|
||||
#ifdef USE_SEPDISTANCE_UTIL2
|
||||
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
|
||||
if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist>SIMD_EPSILON))
|
||||
{
|
||||
m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0->getWorldTransform(),body1->getWorldTransform());
|
||||
}
|
||||
|
||||
@ -29,6 +29,12 @@ Written by: Marcus Hennix
|
||||
#define CONETWIST_DEF_FIX_THRESH btScalar(.05f)
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis, const btMatrix3x3& invInertiaWorld)
|
||||
{
|
||||
btVector3 vec = axis * invInertiaWorld;
|
||||
return axis.dot(vec);
|
||||
}
|
||||
|
||||
|
||||
btConeTwistConstraint::btConeTwistConstraint()
|
||||
:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE),
|
||||
@ -70,8 +76,6 @@ void btConeTwistConstraint::init()
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
|
||||
{
|
||||
if (m_useSolveConstraintObsolete)
|
||||
@ -83,7 +87,7 @@ void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
|
||||
{
|
||||
info->m_numConstraintRows = 3;
|
||||
info->nub = 3;
|
||||
calcAngleInfo2();
|
||||
calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
|
||||
if(m_solveSwingLimit)
|
||||
{
|
||||
info->m_numConstraintRows++;
|
||||
@ -101,22 +105,31 @@ void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void btConeTwistConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
|
||||
{
|
||||
//always reserve 6 rows: object transform is not available on SPU
|
||||
info->m_numConstraintRows = 6;
|
||||
info->nub = 0;
|
||||
|
||||
}
|
||||
|
||||
|
||||
void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info)
|
||||
{
|
||||
getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
|
||||
}
|
||||
|
||||
void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB)
|
||||
{
|
||||
calcAngleInfo2(transA,transB,invInertiaWorldA,invInertiaWorldB);
|
||||
|
||||
btAssert(!m_useSolveConstraintObsolete);
|
||||
//retrieve matrices
|
||||
btTransform body0_trans;
|
||||
body0_trans = m_rbA.getCenterOfMassTransform();
|
||||
btTransform body1_trans;
|
||||
body1_trans = m_rbB.getCenterOfMassTransform();
|
||||
// set jacobian
|
||||
info->m_J1linearAxis[0] = 1;
|
||||
info->m_J1linearAxis[info->rowskip+1] = 1;
|
||||
info->m_J1linearAxis[2*info->rowskip+2] = 1;
|
||||
btVector3 a1 = body0_trans.getBasis() * m_rbAFrame.getOrigin();
|
||||
btVector3 a1 = transA.getBasis() * m_rbAFrame.getOrigin();
|
||||
{
|
||||
btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
|
||||
btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
|
||||
@ -124,7 +137,7 @@ void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info)
|
||||
btVector3 a1neg = -a1;
|
||||
a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
|
||||
}
|
||||
btVector3 a2 = body1_trans.getBasis() * m_rbBFrame.getOrigin();
|
||||
btVector3 a2 = transB.getBasis() * m_rbBFrame.getOrigin();
|
||||
{
|
||||
btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
|
||||
btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
|
||||
@ -136,7 +149,7 @@ void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info)
|
||||
int j;
|
||||
for (j=0; j<3; j++)
|
||||
{
|
||||
info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
|
||||
info->m_constraintError[j*info->rowskip] = k * (a2[j] + transB.getOrigin()[j] - a1[j] - transA.getOrigin()[j]);
|
||||
info->m_lowerLimit[j*info->rowskip] = -SIMD_INFINITY;
|
||||
info->m_upperLimit[j*info->rowskip] = SIMD_INFINITY;
|
||||
}
|
||||
@ -275,7 +288,7 @@ void btConeTwistConstraint::buildJacobian()
|
||||
}
|
||||
}
|
||||
|
||||
calcAngleInfo2();
|
||||
calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
|
||||
}
|
||||
}
|
||||
|
||||
@ -593,7 +606,7 @@ static btVector3 vTwist(1,0,0); // twist axis in constraint's space
|
||||
|
||||
|
||||
|
||||
void btConeTwistConstraint::calcAngleInfo2()
|
||||
void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB)
|
||||
{
|
||||
m_swingCorrection = btScalar(0.);
|
||||
m_twistLimitSign = btScalar(0.);
|
||||
@ -606,8 +619,8 @@ void btConeTwistConstraint::calcAngleInfo2()
|
||||
// TODO : split rotation to pure swing and pure twist
|
||||
// compute desired transforms in world
|
||||
btTransform trPose(m_qTarget);
|
||||
btTransform trA = getRigidBodyA().getCenterOfMassTransform() * m_rbAFrame;
|
||||
btTransform trB = getRigidBodyB().getCenterOfMassTransform() * m_rbBFrame;
|
||||
btTransform trA = transA * m_rbAFrame;
|
||||
btTransform trB = transB * m_rbBFrame;
|
||||
btTransform trDeltaAB = trB * trPose * trA.inverse();
|
||||
btQuaternion qDeltaAB = trDeltaAB.getRotation();
|
||||
btVector3 swingAxis = btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z());
|
||||
@ -624,8 +637,8 @@ void btConeTwistConstraint::calcAngleInfo2()
|
||||
|
||||
{
|
||||
// compute rotation of A wrt B (in constraint space)
|
||||
btQuaternion qA = getRigidBodyA().getCenterOfMassTransform().getRotation() * m_rbAFrame.getRotation();
|
||||
btQuaternion qB = getRigidBodyB().getCenterOfMassTransform().getRotation() * m_rbBFrame.getRotation();
|
||||
btQuaternion qA = transA.getRotation() * m_rbAFrame.getRotation();
|
||||
btQuaternion qB = transB.getRotation() * m_rbBFrame.getRotation();
|
||||
btQuaternion qAB = qB.inverse() * qA;
|
||||
// split rotation into cone and twist
|
||||
// (all this is done from B's perspective. Maybe I should be averaging axes...)
|
||||
@ -673,10 +686,10 @@ void btConeTwistConstraint::calcAngleInfo2()
|
||||
// you haven't set any limits;
|
||||
// or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?)
|
||||
// anyway, we have either hinge or fixed joint
|
||||
btVector3 ivA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0);
|
||||
btVector3 jvA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1);
|
||||
btVector3 kvA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
|
||||
btVector3 ivB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(0);
|
||||
btVector3 ivA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
|
||||
btVector3 jvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1);
|
||||
btVector3 kvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(2);
|
||||
btVector3 ivB = transB.getBasis() * m_rbBFrame.getBasis().getColumn(0);
|
||||
btVector3 target;
|
||||
btScalar x = ivB.dot(ivA);
|
||||
btScalar y = ivB.dot(jvA);
|
||||
|
||||
@ -110,9 +110,12 @@ public:
|
||||
virtual void buildJacobian();
|
||||
|
||||
virtual void getInfo1 (btConstraintInfo1* info);
|
||||
|
||||
void getInfo1NonVirtual(btConstraintInfo1* info);
|
||||
|
||||
virtual void getInfo2 (btConstraintInfo2* info);
|
||||
|
||||
void getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB);
|
||||
|
||||
virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep);
|
||||
|
||||
@ -198,7 +201,7 @@ public:
|
||||
}
|
||||
|
||||
void calcAngleInfo();
|
||||
void calcAngleInfo2();
|
||||
void calcAngleInfo2(const btTransform& transA, const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB);
|
||||
|
||||
inline btScalar getSwingSpan1()
|
||||
{
|
||||
|
||||
@ -14,6 +14,59 @@ subject to the following restrictions:
|
||||
*/
|
||||
|
||||
|
||||
#include "btContactConstraint.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "btJacobianEntry.h"
|
||||
#include "btContactSolverInfo.h"
|
||||
#include "LinearMath/btMinMax.h"
|
||||
#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
|
||||
|
||||
btContactConstraint::btContactConstraint()
|
||||
:btTypedConstraint(CONTACT_CONSTRAINT_TYPE)
|
||||
{
|
||||
}
|
||||
|
||||
btContactConstraint::btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB)
|
||||
:btTypedConstraint(CONTACT_CONSTRAINT_TYPE,rbA,rbB),
|
||||
m_contactManifold(*contactManifold)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
btContactConstraint::~btContactConstraint()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
void btContactConstraint::setContactManifold(btPersistentManifold* contactManifold)
|
||||
{
|
||||
m_contactManifold = *contactManifold;
|
||||
}
|
||||
|
||||
void btContactConstraint::getInfo1 (btConstraintInfo1* info)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
void btContactConstraint::getInfo2 (btConstraintInfo2* info)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
void btContactConstraint::buildJacobian()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
void btContactConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
#include "btContactConstraint.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "LinearMath/btVector3.h"
|
||||
@ -85,345 +138,4 @@ void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
|
||||
|
||||
|
||||
|
||||
//response between two dynamic objects with friction
|
||||
btScalar resolveSingleCollision(
|
||||
btRigidBody& body1,
|
||||
btRigidBody& body2,
|
||||
btManifoldPoint& contactPoint,
|
||||
const btContactSolverInfo& solverInfo)
|
||||
{
|
||||
|
||||
const btVector3& pos1_ = contactPoint.getPositionWorldOnA();
|
||||
const btVector3& pos2_ = contactPoint.getPositionWorldOnB();
|
||||
const btVector3& normal = contactPoint.m_normalWorldOnB;
|
||||
|
||||
//constant over all iterations
|
||||
btVector3 rel_pos1 = pos1_ - body1.getCenterOfMassPosition();
|
||||
btVector3 rel_pos2 = pos2_ - body2.getCenterOfMassPosition();
|
||||
|
||||
btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
|
||||
btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
|
||||
btVector3 vel = vel1 - vel2;
|
||||
btScalar rel_vel;
|
||||
rel_vel = normal.dot(vel);
|
||||
|
||||
btScalar Kfps = btScalar(1.) / solverInfo.m_timeStep ;
|
||||
|
||||
// btScalar damping = solverInfo.m_damping ;
|
||||
btScalar Kerp = solverInfo.m_erp;
|
||||
btScalar Kcor = Kerp *Kfps;
|
||||
|
||||
btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
|
||||
btAssert(cpd);
|
||||
btScalar distance = cpd->m_penetration;
|
||||
btScalar positionalError = Kcor *-distance;
|
||||
btScalar velocityError = cpd->m_restitution - rel_vel;// * damping;
|
||||
|
||||
btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;
|
||||
|
||||
btScalar velocityImpulse = velocityError * cpd->m_jacDiagABInv;
|
||||
|
||||
btScalar normalImpulse = penetrationImpulse+velocityImpulse;
|
||||
|
||||
// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
|
||||
btScalar oldNormalImpulse = cpd->m_appliedImpulse;
|
||||
btScalar sum = oldNormalImpulse + normalImpulse;
|
||||
cpd->m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
|
||||
|
||||
normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse;
|
||||
|
||||
#ifdef USE_INTERNAL_APPLY_IMPULSE
|
||||
if (body1.getInvMass())
|
||||
{
|
||||
body1.internalApplyImpulse(contactPoint.m_normalWorldOnB*body1.getInvMass(),cpd->m_angularComponentA,normalImpulse);
|
||||
}
|
||||
if (body2.getInvMass())
|
||||
{
|
||||
body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse);
|
||||
}
|
||||
#else //USE_INTERNAL_APPLY_IMPULSE
|
||||
body1.applyImpulse(normal*(normalImpulse), rel_pos1);
|
||||
body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
|
||||
#endif //USE_INTERNAL_APPLY_IMPULSE
|
||||
|
||||
return normalImpulse;
|
||||
}
|
||||
|
||||
|
||||
btScalar resolveSingleFriction(
|
||||
btRigidBody& body1,
|
||||
btRigidBody& body2,
|
||||
btManifoldPoint& contactPoint,
|
||||
const btContactSolverInfo& solverInfo)
|
||||
{
|
||||
|
||||
(void)solverInfo;
|
||||
|
||||
const btVector3& pos1 = contactPoint.getPositionWorldOnA();
|
||||
const btVector3& pos2 = contactPoint.getPositionWorldOnB();
|
||||
|
||||
btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
|
||||
btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
|
||||
|
||||
btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
|
||||
btAssert(cpd);
|
||||
|
||||
btScalar combinedFriction = cpd->m_friction;
|
||||
|
||||
btScalar limit = cpd->m_appliedImpulse * combinedFriction;
|
||||
|
||||
if (cpd->m_appliedImpulse>btScalar(0.))
|
||||
//friction
|
||||
{
|
||||
//apply friction in the 2 tangential directions
|
||||
|
||||
// 1st tangent
|
||||
btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
|
||||
btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
|
||||
btVector3 vel = vel1 - vel2;
|
||||
|
||||
btScalar j1,j2;
|
||||
|
||||
{
|
||||
|
||||
btScalar vrel = cpd->m_frictionWorldTangential0.dot(vel);
|
||||
|
||||
// calculate j that moves us to zero relative velocity
|
||||
j1 = -vrel * cpd->m_jacDiagABInvTangent0;
|
||||
btScalar oldTangentImpulse = cpd->m_accumulatedTangentImpulse0;
|
||||
cpd->m_accumulatedTangentImpulse0 = oldTangentImpulse + j1;
|
||||
btSetMin(cpd->m_accumulatedTangentImpulse0, limit);
|
||||
btSetMax(cpd->m_accumulatedTangentImpulse0, -limit);
|
||||
j1 = cpd->m_accumulatedTangentImpulse0 - oldTangentImpulse;
|
||||
|
||||
}
|
||||
{
|
||||
// 2nd tangent
|
||||
|
||||
btScalar vrel = cpd->m_frictionWorldTangential1.dot(vel);
|
||||
|
||||
// calculate j that moves us to zero relative velocity
|
||||
j2 = -vrel * cpd->m_jacDiagABInvTangent1;
|
||||
btScalar oldTangentImpulse = cpd->m_accumulatedTangentImpulse1;
|
||||
cpd->m_accumulatedTangentImpulse1 = oldTangentImpulse + j2;
|
||||
btSetMin(cpd->m_accumulatedTangentImpulse1, limit);
|
||||
btSetMax(cpd->m_accumulatedTangentImpulse1, -limit);
|
||||
j2 = cpd->m_accumulatedTangentImpulse1 - oldTangentImpulse;
|
||||
}
|
||||
|
||||
#ifdef USE_INTERNAL_APPLY_IMPULSE
|
||||
if (body1.getInvMass())
|
||||
{
|
||||
body1.internalApplyImpulse(cpd->m_frictionWorldTangential0*body1.getInvMass(),cpd->m_frictionAngularComponent0A,j1);
|
||||
body1.internalApplyImpulse(cpd->m_frictionWorldTangential1*body1.getInvMass(),cpd->m_frictionAngularComponent1A,j2);
|
||||
}
|
||||
if (body2.getInvMass())
|
||||
{
|
||||
body2.internalApplyImpulse(cpd->m_frictionWorldTangential0*body2.getInvMass(),cpd->m_frictionAngularComponent0B,-j1);
|
||||
body2.internalApplyImpulse(cpd->m_frictionWorldTangential1*body2.getInvMass(),cpd->m_frictionAngularComponent1B,-j2);
|
||||
}
|
||||
#else //USE_INTERNAL_APPLY_IMPULSE
|
||||
body1.applyImpulse((j1 * cpd->m_frictionWorldTangential0)+(j2 * cpd->m_frictionWorldTangential1), rel_pos1);
|
||||
body2.applyImpulse((j1 * -cpd->m_frictionWorldTangential0)+(j2 * -cpd->m_frictionWorldTangential1), rel_pos2);
|
||||
#endif //USE_INTERNAL_APPLY_IMPULSE
|
||||
|
||||
|
||||
}
|
||||
return cpd->m_appliedImpulse;
|
||||
}
|
||||
|
||||
|
||||
btScalar resolveSingleFrictionOriginal(
|
||||
btRigidBody& body1,
|
||||
btRigidBody& body2,
|
||||
btManifoldPoint& contactPoint,
|
||||
const btContactSolverInfo& solverInfo);
|
||||
|
||||
btScalar resolveSingleFrictionOriginal(
|
||||
btRigidBody& body1,
|
||||
btRigidBody& body2,
|
||||
btManifoldPoint& contactPoint,
|
||||
const btContactSolverInfo& solverInfo)
|
||||
{
|
||||
|
||||
(void)solverInfo;
|
||||
|
||||
const btVector3& pos1 = contactPoint.getPositionWorldOnA();
|
||||
const btVector3& pos2 = contactPoint.getPositionWorldOnB();
|
||||
|
||||
btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
|
||||
btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
|
||||
|
||||
btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
|
||||
btAssert(cpd);
|
||||
|
||||
btScalar combinedFriction = cpd->m_friction;
|
||||
|
||||
btScalar limit = cpd->m_appliedImpulse * combinedFriction;
|
||||
//if (contactPoint.m_appliedImpulse>btScalar(0.))
|
||||
//friction
|
||||
{
|
||||
//apply friction in the 2 tangential directions
|
||||
|
||||
{
|
||||
// 1st tangent
|
||||
btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
|
||||
btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
|
||||
btVector3 vel = vel1 - vel2;
|
||||
|
||||
btScalar vrel = cpd->m_frictionWorldTangential0.dot(vel);
|
||||
|
||||
// calculate j that moves us to zero relative velocity
|
||||
btScalar j = -vrel * cpd->m_jacDiagABInvTangent0;
|
||||
btScalar total = cpd->m_accumulatedTangentImpulse0 + j;
|
||||
btSetMin(total, limit);
|
||||
btSetMax(total, -limit);
|
||||
j = total - cpd->m_accumulatedTangentImpulse0;
|
||||
cpd->m_accumulatedTangentImpulse0 = total;
|
||||
body1.applyImpulse(j * cpd->m_frictionWorldTangential0, rel_pos1);
|
||||
body2.applyImpulse(j * -cpd->m_frictionWorldTangential0, rel_pos2);
|
||||
}
|
||||
|
||||
|
||||
{
|
||||
// 2nd tangent
|
||||
btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
|
||||
btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
|
||||
btVector3 vel = vel1 - vel2;
|
||||
|
||||
btScalar vrel = cpd->m_frictionWorldTangential1.dot(vel);
|
||||
|
||||
// calculate j that moves us to zero relative velocity
|
||||
btScalar j = -vrel * cpd->m_jacDiagABInvTangent1;
|
||||
btScalar total = cpd->m_accumulatedTangentImpulse1 + j;
|
||||
btSetMin(total, limit);
|
||||
btSetMax(total, -limit);
|
||||
j = total - cpd->m_accumulatedTangentImpulse1;
|
||||
cpd->m_accumulatedTangentImpulse1 = total;
|
||||
body1.applyImpulse(j * cpd->m_frictionWorldTangential1, rel_pos1);
|
||||
body2.applyImpulse(j * -cpd->m_frictionWorldTangential1, rel_pos2);
|
||||
}
|
||||
}
|
||||
return cpd->m_appliedImpulse;
|
||||
}
|
||||
|
||||
|
||||
//velocity + friction
|
||||
//response between two dynamic objects with friction
|
||||
btScalar resolveSingleCollisionCombined(
|
||||
btRigidBody& body1,
|
||||
btRigidBody& body2,
|
||||
btManifoldPoint& contactPoint,
|
||||
const btContactSolverInfo& solverInfo)
|
||||
{
|
||||
|
||||
const btVector3& pos1 = contactPoint.getPositionWorldOnA();
|
||||
const btVector3& pos2 = contactPoint.getPositionWorldOnB();
|
||||
const btVector3& normal = contactPoint.m_normalWorldOnB;
|
||||
|
||||
btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
|
||||
btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
|
||||
|
||||
btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
|
||||
btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
|
||||
btVector3 vel = vel1 - vel2;
|
||||
btScalar rel_vel;
|
||||
rel_vel = normal.dot(vel);
|
||||
|
||||
btScalar Kfps = btScalar(1.) / solverInfo.m_timeStep ;
|
||||
|
||||
//btScalar damping = solverInfo.m_damping ;
|
||||
btScalar Kerp = solverInfo.m_erp;
|
||||
btScalar Kcor = Kerp *Kfps;
|
||||
|
||||
btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
|
||||
btAssert(cpd);
|
||||
btScalar distance = cpd->m_penetration;
|
||||
btScalar positionalError = Kcor *-distance;
|
||||
btScalar velocityError = cpd->m_restitution - rel_vel;// * damping;
|
||||
|
||||
btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;
|
||||
|
||||
btScalar velocityImpulse = velocityError * cpd->m_jacDiagABInv;
|
||||
|
||||
btScalar normalImpulse = penetrationImpulse+velocityImpulse;
|
||||
|
||||
// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
|
||||
btScalar oldNormalImpulse = cpd->m_appliedImpulse;
|
||||
btScalar sum = oldNormalImpulse + normalImpulse;
|
||||
cpd->m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
|
||||
|
||||
normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse;
|
||||
|
||||
|
||||
#ifdef USE_INTERNAL_APPLY_IMPULSE
|
||||
if (body1.getInvMass())
|
||||
{
|
||||
body1.internalApplyImpulse(contactPoint.m_normalWorldOnB*body1.getInvMass(),cpd->m_angularComponentA,normalImpulse);
|
||||
}
|
||||
if (body2.getInvMass())
|
||||
{
|
||||
body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse);
|
||||
}
|
||||
#else //USE_INTERNAL_APPLY_IMPULSE
|
||||
body1.applyImpulse(normal*(normalImpulse), rel_pos1);
|
||||
body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
|
||||
#endif //USE_INTERNAL_APPLY_IMPULSE
|
||||
|
||||
{
|
||||
//friction
|
||||
btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
|
||||
btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
|
||||
btVector3 vel = vel1 - vel2;
|
||||
|
||||
rel_vel = normal.dot(vel);
|
||||
|
||||
|
||||
btVector3 lat_vel = vel - normal * rel_vel;
|
||||
btScalar lat_rel_vel = lat_vel.length();
|
||||
|
||||
btScalar combinedFriction = cpd->m_friction;
|
||||
|
||||
if (cpd->m_appliedImpulse > 0)
|
||||
if (lat_rel_vel > SIMD_EPSILON)
|
||||
{
|
||||
lat_vel /= lat_rel_vel;
|
||||
btVector3 temp1 = body1.getInvInertiaTensorWorld() * rel_pos1.cross(lat_vel);
|
||||
btVector3 temp2 = body2.getInvInertiaTensorWorld() * rel_pos2.cross(lat_vel);
|
||||
btScalar friction_impulse = lat_rel_vel /
|
||||
(body1.getInvMass() + body2.getInvMass() + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
|
||||
btScalar normal_impulse = cpd->m_appliedImpulse * combinedFriction;
|
||||
|
||||
btSetMin(friction_impulse, normal_impulse);
|
||||
btSetMax(friction_impulse, -normal_impulse);
|
||||
body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1);
|
||||
body2.applyImpulse(lat_vel * friction_impulse, rel_pos2);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
return normalImpulse;
|
||||
}
|
||||
|
||||
btScalar resolveSingleFrictionEmpty(
|
||||
btRigidBody& body1,
|
||||
btRigidBody& body2,
|
||||
btManifoldPoint& contactPoint,
|
||||
const btContactSolverInfo& solverInfo);
|
||||
|
||||
btScalar resolveSingleFrictionEmpty(
|
||||
btRigidBody& body1,
|
||||
btRigidBody& body2,
|
||||
btManifoldPoint& contactPoint,
|
||||
const btContactSolverInfo& solverInfo)
|
||||
{
|
||||
(void)contactPoint;
|
||||
(void)body1;
|
||||
(void)body2;
|
||||
(void)solverInfo;
|
||||
|
||||
|
||||
return btScalar(0.);
|
||||
}
|
||||
|
||||
|
||||
@ -16,107 +16,56 @@ subject to the following restrictions:
|
||||
#ifndef CONTACT_CONSTRAINT_H
|
||||
#define CONTACT_CONSTRAINT_H
|
||||
|
||||
///@todo: make into a proper class working with the iterative constraint solver
|
||||
|
||||
class btRigidBody;
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "LinearMath/btScalar.h"
|
||||
struct btContactSolverInfo;
|
||||
class btManifoldPoint;
|
||||
#include "btJacobianEntry.h"
|
||||
#include "btTypedConstraint.h"
|
||||
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
|
||||
|
||||
enum {
|
||||
DEFAULT_CONTACT_SOLVER_TYPE=0,
|
||||
CONTACT_SOLVER_TYPE1,
|
||||
CONTACT_SOLVER_TYPE2,
|
||||
USER_CONTACT_SOLVER_TYPE1,
|
||||
MAX_CONTACT_SOLVER_TYPES
|
||||
};
|
||||
|
||||
|
||||
typedef btScalar (*ContactSolverFunc)(btRigidBody& body1,
|
||||
btRigidBody& body2,
|
||||
class btManifoldPoint& contactPoint,
|
||||
const btContactSolverInfo& info);
|
||||
|
||||
///stores some extra information to each contact point. It is not in the contact point, because that want to keep the collision detection independent from the constraint solver.
|
||||
struct btConstraintPersistentData
|
||||
///btContactConstraint can be automatically created to solve contact constraints using the unified btTypedConstraint interface
|
||||
ATTRIBUTE_ALIGNED16(class) btContactConstraint : public btTypedConstraint
|
||||
{
|
||||
inline btConstraintPersistentData()
|
||||
:m_appliedImpulse(btScalar(0.)),
|
||||
m_prevAppliedImpulse(btScalar(0.)),
|
||||
m_accumulatedTangentImpulse0(btScalar(0.)),
|
||||
m_accumulatedTangentImpulse1(btScalar(0.)),
|
||||
m_jacDiagABInv(btScalar(0.)),
|
||||
m_persistentLifeTime(0),
|
||||
m_restitution(btScalar(0.)),
|
||||
m_friction(btScalar(0.)),
|
||||
m_penetration(btScalar(0.)),
|
||||
m_contactSolverFunc(0),
|
||||
m_frictionSolverFunc(0)
|
||||
protected:
|
||||
|
||||
btPersistentManifold m_contactManifold;
|
||||
|
||||
public:
|
||||
|
||||
btContactConstraint();
|
||||
|
||||
btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB);
|
||||
|
||||
void setContactManifold(btPersistentManifold* contactManifold);
|
||||
|
||||
btPersistentManifold* getContactManifold()
|
||||
{
|
||||
return &m_contactManifold;
|
||||
}
|
||||
|
||||
|
||||
/// total applied impulse during most recent frame
|
||||
btScalar m_appliedImpulse;
|
||||
btScalar m_prevAppliedImpulse;
|
||||
btScalar m_accumulatedTangentImpulse0;
|
||||
btScalar m_accumulatedTangentImpulse1;
|
||||
|
||||
btScalar m_jacDiagABInv;
|
||||
btScalar m_jacDiagABInvTangent0;
|
||||
btScalar m_jacDiagABInvTangent1;
|
||||
int m_persistentLifeTime;
|
||||
btScalar m_restitution;
|
||||
btScalar m_friction;
|
||||
btScalar m_penetration;
|
||||
btVector3 m_frictionWorldTangential0;
|
||||
btVector3 m_frictionWorldTangential1;
|
||||
|
||||
btVector3 m_frictionAngularComponent0A;
|
||||
btVector3 m_frictionAngularComponent0B;
|
||||
btVector3 m_frictionAngularComponent1A;
|
||||
btVector3 m_frictionAngularComponent1B;
|
||||
const btPersistentManifold* getContactManifold() const
|
||||
{
|
||||
return &m_contactManifold;
|
||||
}
|
||||
|
||||
//some data doesn't need to be persistent over frames: todo: clean/reuse this
|
||||
btVector3 m_angularComponentA;
|
||||
btVector3 m_angularComponentB;
|
||||
|
||||
ContactSolverFunc m_contactSolverFunc;
|
||||
ContactSolverFunc m_frictionSolverFunc;
|
||||
virtual ~btContactConstraint();
|
||||
|
||||
virtual void getInfo1 (btConstraintInfo1* info);
|
||||
|
||||
virtual void getInfo2 (btConstraintInfo2* info);
|
||||
|
||||
///obsolete methods
|
||||
virtual void buildJacobian();
|
||||
|
||||
///obsolete methods
|
||||
virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep);
|
||||
|
||||
};
|
||||
|
||||
///bilateral constraint between two dynamic objects
|
||||
///positive distance = separation, negative distance = penetration
|
||||
|
||||
///resolveSingleBilateral is an obsolete methods used for vehicle friction between two dynamic objects
|
||||
void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
|
||||
btRigidBody& body2, const btVector3& pos2,
|
||||
btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep);
|
||||
|
||||
|
||||
///contact constraint resolution:
|
||||
///calculate and apply impulse to satisfy non-penetration and non-negative relative velocity constraint
|
||||
///positive distance = separation, negative distance = penetration
|
||||
btScalar resolveSingleCollision(
|
||||
btRigidBody& body1,
|
||||
btRigidBody& body2,
|
||||
btManifoldPoint& contactPoint,
|
||||
const btContactSolverInfo& info);
|
||||
|
||||
btScalar resolveSingleFriction(
|
||||
btRigidBody& body1,
|
||||
btRigidBody& body2,
|
||||
btManifoldPoint& contactPoint,
|
||||
const btContactSolverInfo& solverInfo
|
||||
);
|
||||
|
||||
|
||||
|
||||
btScalar resolveSingleCollisionCombined(
|
||||
btRigidBody& body1,
|
||||
btRigidBody& body2,
|
||||
btManifoldPoint& contactPoint,
|
||||
const btContactSolverInfo& solverInfo
|
||||
);
|
||||
|
||||
#endif //CONTACT_CONSTRAINT_H
|
||||
|
||||
@ -26,7 +26,7 @@ subject to the following restrictions:
|
||||
#define HINGE_USE_OBSOLETE_SOLVER false
|
||||
|
||||
|
||||
|
||||
#ifndef __SPU__
|
||||
|
||||
btHingeConstraint::btHingeConstraint()
|
||||
: btTypedConstraint (HINGE_CONSTRAINT_TYPE),
|
||||
@ -249,7 +249,7 @@ void btHingeConstraint::buildJacobian()
|
||||
m_accLimitImpulse = btScalar(0.);
|
||||
|
||||
// test angular limit
|
||||
testLimit();
|
||||
testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
|
||||
|
||||
//Compute K = J*W*J' for hinge axis
|
||||
btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
|
||||
@ -259,6 +259,166 @@ void btHingeConstraint::buildJacobian()
|
||||
}
|
||||
}
|
||||
|
||||
void btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
|
||||
{
|
||||
|
||||
///for backwards compatibility during the transition to 'getInfo/getInfo2'
|
||||
if (m_useSolveConstraintObsolete)
|
||||
{
|
||||
|
||||
btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
|
||||
btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
|
||||
|
||||
btScalar tau = btScalar(0.3);
|
||||
|
||||
//linear part
|
||||
if (!m_angularOnly)
|
||||
{
|
||||
btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
|
||||
btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
|
||||
|
||||
btVector3 vel1,vel2;
|
||||
bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
|
||||
bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
|
||||
btVector3 vel = vel1 - vel2;
|
||||
|
||||
for (int i=0;i<3;i++)
|
||||
{
|
||||
const btVector3& normal = m_jac[i].m_linearJointAxis;
|
||||
btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
|
||||
|
||||
btScalar rel_vel;
|
||||
rel_vel = normal.dot(vel);
|
||||
//positional error (zeroth order error)
|
||||
btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
|
||||
btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv;
|
||||
m_appliedImpulse += impulse;
|
||||
btVector3 impulse_vector = normal * impulse;
|
||||
btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
|
||||
btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
|
||||
bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
|
||||
bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
{
|
||||
///solve angular part
|
||||
|
||||
// get axes in world space
|
||||
btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
|
||||
btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(2);
|
||||
|
||||
btVector3 angVelA;
|
||||
bodyA.getAngularVelocity(angVelA);
|
||||
btVector3 angVelB;
|
||||
bodyB.getAngularVelocity(angVelB);
|
||||
|
||||
btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA);
|
||||
btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB);
|
||||
|
||||
btVector3 angAorthog = angVelA - angVelAroundHingeAxisA;
|
||||
btVector3 angBorthog = angVelB - angVelAroundHingeAxisB;
|
||||
btVector3 velrelOrthog = angAorthog-angBorthog;
|
||||
{
|
||||
|
||||
|
||||
//solve orthogonal angular velocity correction
|
||||
//btScalar relaxation = btScalar(1.);
|
||||
btScalar len = velrelOrthog.length();
|
||||
if (len > btScalar(0.00001))
|
||||
{
|
||||
btVector3 normal = velrelOrthog.normalized();
|
||||
btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) +
|
||||
getRigidBodyB().computeAngularImpulseDenominator(normal);
|
||||
// scale for mass and relaxation
|
||||
//velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor;
|
||||
|
||||
bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*velrelOrthog,-(btScalar(1.)/denom));
|
||||
bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*velrelOrthog,(btScalar(1.)/denom));
|
||||
|
||||
}
|
||||
|
||||
//solve angular positional correction
|
||||
btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/timeStep);
|
||||
btScalar len2 = angularError.length();
|
||||
if (len2>btScalar(0.00001))
|
||||
{
|
||||
btVector3 normal2 = angularError.normalized();
|
||||
btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) +
|
||||
getRigidBodyB().computeAngularImpulseDenominator(normal2);
|
||||
//angularError *= (btScalar(1.)/denom2) * relaxation;
|
||||
|
||||
bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*angularError,(btScalar(1.)/denom2));
|
||||
bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*angularError,-(btScalar(1.)/denom2));
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// solve limit
|
||||
if (m_solveLimit)
|
||||
{
|
||||
btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor ) * m_limitSign;
|
||||
|
||||
btScalar impulseMag = amplitude * m_kHinge;
|
||||
|
||||
// Clamp the accumulated impulse
|
||||
btScalar temp = m_accLimitImpulse;
|
||||
m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) );
|
||||
impulseMag = m_accLimitImpulse - temp;
|
||||
|
||||
|
||||
|
||||
bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,impulseMag * m_limitSign);
|
||||
bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-(impulseMag * m_limitSign));
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
//apply motor
|
||||
if (m_enableAngularMotor)
|
||||
{
|
||||
//todo: add limits too
|
||||
btVector3 angularLimit(0,0,0);
|
||||
|
||||
btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
|
||||
btScalar projRelVel = velrel.dot(axisA);
|
||||
|
||||
btScalar desiredMotorVel = m_motorTargetVelocity;
|
||||
btScalar motor_relvel = desiredMotorVel - projRelVel;
|
||||
|
||||
btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;;
|
||||
|
||||
// accumulated impulse clipping:
|
||||
btScalar fMaxImpulse = m_maxMotorImpulse;
|
||||
btScalar newAccImpulse = m_accMotorImpulse + unclippedMotorImpulse;
|
||||
btScalar clippedMotorImpulse = unclippedMotorImpulse;
|
||||
if (newAccImpulse > fMaxImpulse)
|
||||
{
|
||||
newAccImpulse = fMaxImpulse;
|
||||
clippedMotorImpulse = newAccImpulse - m_accMotorImpulse;
|
||||
}
|
||||
else if (newAccImpulse < -fMaxImpulse)
|
||||
{
|
||||
newAccImpulse = -fMaxImpulse;
|
||||
clippedMotorImpulse = newAccImpulse - m_accMotorImpulse;
|
||||
}
|
||||
m_accMotorImpulse += clippedMotorImpulse;
|
||||
|
||||
bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,clippedMotorImpulse);
|
||||
bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-clippedMotorImpulse);
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
#endif //__SPU__
|
||||
|
||||
|
||||
void btHingeConstraint::getInfo1(btConstraintInfo1* info)
|
||||
@ -272,52 +432,77 @@ void btHingeConstraint::getInfo1(btConstraintInfo1* info)
|
||||
{
|
||||
info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular
|
||||
info->nub = 1;
|
||||
//always add the row, to avoid computation (data is not available yet)
|
||||
//prepare constraint
|
||||
testLimit();
|
||||
testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
|
||||
if(getSolveLimit() || getEnableAngularMotor())
|
||||
{
|
||||
info->m_numConstraintRows++; // limit 3rd anguar as well
|
||||
info->nub--;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void btHingeConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
|
||||
{
|
||||
if (m_useSolveConstraintObsolete)
|
||||
{
|
||||
info->m_numConstraintRows = 0;
|
||||
info->nub = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
//always add the 'limit' row, to avoid computation (data is not available yet)
|
||||
info->m_numConstraintRows = 6; // Fixed 3 linear + 2 angular
|
||||
info->nub = 0;
|
||||
}
|
||||
}
|
||||
|
||||
void btHingeConstraint::getInfo2 (btConstraintInfo2* info)
|
||||
{
|
||||
getInfo2NonVirtual(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity());
|
||||
}
|
||||
|
||||
void btHingeConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
|
||||
{
|
||||
btAssert(!m_useSolveConstraintObsolete);
|
||||
int i, s = info->rowskip;
|
||||
int i, skip = info->rowskip;
|
||||
// transforms in world space
|
||||
btTransform trA = m_rbA.getCenterOfMassTransform()*m_rbAFrame;
|
||||
btTransform trB = m_rbB.getCenterOfMassTransform()*m_rbBFrame;
|
||||
btTransform trA = transA*m_rbAFrame;
|
||||
btTransform trB = transB*m_rbBFrame;
|
||||
// pivot point
|
||||
btVector3 pivotAInW = trA.getOrigin();
|
||||
btVector3 pivotBInW = trB.getOrigin();
|
||||
|
||||
// linear (all fixed)
|
||||
info->m_J1linearAxis[0] = 1;
|
||||
info->m_J1linearAxis[s + 1] = 1;
|
||||
info->m_J1linearAxis[2 * s + 2] = 1;
|
||||
btVector3 a1 = pivotAInW - m_rbA.getCenterOfMassTransform().getOrigin();
|
||||
info->m_J1linearAxis[skip + 1] = 1;
|
||||
info->m_J1linearAxis[2 * skip + 2] = 1;
|
||||
|
||||
|
||||
|
||||
|
||||
btVector3 a1 = pivotAInW - transA.getOrigin();
|
||||
{
|
||||
btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
|
||||
btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + s);
|
||||
btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * s);
|
||||
btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + skip);
|
||||
btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * skip);
|
||||
btVector3 a1neg = -a1;
|
||||
a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
|
||||
}
|
||||
btVector3 a2 = pivotBInW - m_rbB.getCenterOfMassTransform().getOrigin();
|
||||
btVector3 a2 = pivotBInW - transB.getOrigin();
|
||||
{
|
||||
btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
|
||||
btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + s);
|
||||
btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * s);
|
||||
btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + skip);
|
||||
btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * skip);
|
||||
a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
|
||||
}
|
||||
// linear RHS
|
||||
btScalar k = info->fps * info->erp;
|
||||
for(i = 0; i < 3; i++)
|
||||
{
|
||||
info->m_constraintError[i * s] = k * (pivotBInW[i] - pivotAInW[i]);
|
||||
info->m_constraintError[i * skip] = k * (pivotBInW[i] - pivotAInW[i]);
|
||||
}
|
||||
// make rotations around X and Y equal
|
||||
// the hinge axis should be the only unconstrained
|
||||
@ -436,8 +621,8 @@ void btHingeConstraint::getInfo2 (btConstraintInfo2* info)
|
||||
btScalar bounce = m_relaxationFactor;
|
||||
if(bounce > btScalar(0.0))
|
||||
{
|
||||
btScalar vel = m_rbA.getAngularVelocity().dot(ax1);
|
||||
vel -= m_rbB.getAngularVelocity().dot(ax1);
|
||||
btScalar vel = angVelA.dot(ax1);
|
||||
vel -= angVelB.dot(ax1);
|
||||
// only apply bounce if the velocity is incoming, and if the
|
||||
// resulting c[] exceeds what we already have.
|
||||
if(limit == 1)
|
||||
@ -470,163 +655,6 @@ void btHingeConstraint::getInfo2 (btConstraintInfo2* info)
|
||||
|
||||
|
||||
|
||||
void btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
|
||||
{
|
||||
|
||||
///for backwards compatibility during the transition to 'getInfo/getInfo2'
|
||||
if (m_useSolveConstraintObsolete)
|
||||
{
|
||||
|
||||
btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
|
||||
btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
|
||||
|
||||
btScalar tau = btScalar(0.3);
|
||||
|
||||
//linear part
|
||||
if (!m_angularOnly)
|
||||
{
|
||||
btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
|
||||
btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
|
||||
|
||||
btVector3 vel1,vel2;
|
||||
bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
|
||||
bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
|
||||
btVector3 vel = vel1 - vel2;
|
||||
|
||||
for (int i=0;i<3;i++)
|
||||
{
|
||||
const btVector3& normal = m_jac[i].m_linearJointAxis;
|
||||
btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
|
||||
|
||||
btScalar rel_vel;
|
||||
rel_vel = normal.dot(vel);
|
||||
//positional error (zeroth order error)
|
||||
btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
|
||||
btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv;
|
||||
m_appliedImpulse += impulse;
|
||||
btVector3 impulse_vector = normal * impulse;
|
||||
btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
|
||||
btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
|
||||
bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
|
||||
bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
{
|
||||
///solve angular part
|
||||
|
||||
// get axes in world space
|
||||
btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
|
||||
btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(2);
|
||||
|
||||
btVector3 angVelA;
|
||||
bodyA.getAngularVelocity(angVelA);
|
||||
btVector3 angVelB;
|
||||
bodyB.getAngularVelocity(angVelB);
|
||||
|
||||
btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA);
|
||||
btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB);
|
||||
|
||||
btVector3 angAorthog = angVelA - angVelAroundHingeAxisA;
|
||||
btVector3 angBorthog = angVelB - angVelAroundHingeAxisB;
|
||||
btVector3 velrelOrthog = angAorthog-angBorthog;
|
||||
{
|
||||
|
||||
|
||||
//solve orthogonal angular velocity correction
|
||||
btScalar relaxation = btScalar(1.);
|
||||
btScalar len = velrelOrthog.length();
|
||||
if (len > btScalar(0.00001))
|
||||
{
|
||||
btVector3 normal = velrelOrthog.normalized();
|
||||
btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) +
|
||||
getRigidBodyB().computeAngularImpulseDenominator(normal);
|
||||
// scale for mass and relaxation
|
||||
//velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor;
|
||||
|
||||
bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*velrelOrthog,-(btScalar(1.)/denom));
|
||||
bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*velrelOrthog,(btScalar(1.)/denom));
|
||||
|
||||
}
|
||||
|
||||
//solve angular positional correction
|
||||
btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/timeStep);
|
||||
btScalar len2 = angularError.length();
|
||||
if (len2>btScalar(0.00001))
|
||||
{
|
||||
btVector3 normal2 = angularError.normalized();
|
||||
btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) +
|
||||
getRigidBodyB().computeAngularImpulseDenominator(normal2);
|
||||
//angularError *= (btScalar(1.)/denom2) * relaxation;
|
||||
|
||||
bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*angularError,(btScalar(1.)/denom2));
|
||||
bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*angularError,-(btScalar(1.)/denom2));
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// solve limit
|
||||
if (m_solveLimit)
|
||||
{
|
||||
btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor ) * m_limitSign;
|
||||
|
||||
btScalar impulseMag = amplitude * m_kHinge;
|
||||
|
||||
// Clamp the accumulated impulse
|
||||
btScalar temp = m_accLimitImpulse;
|
||||
m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) );
|
||||
impulseMag = m_accLimitImpulse - temp;
|
||||
|
||||
|
||||
|
||||
bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,impulseMag * m_limitSign);
|
||||
bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-(impulseMag * m_limitSign));
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
//apply motor
|
||||
if (m_enableAngularMotor)
|
||||
{
|
||||
//todo: add limits too
|
||||
btVector3 angularLimit(0,0,0);
|
||||
|
||||
btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
|
||||
btScalar projRelVel = velrel.dot(axisA);
|
||||
|
||||
btScalar desiredMotorVel = m_motorTargetVelocity;
|
||||
btScalar motor_relvel = desiredMotorVel - projRelVel;
|
||||
|
||||
btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;;
|
||||
|
||||
// accumulated impulse clipping:
|
||||
btScalar fMaxImpulse = m_maxMotorImpulse;
|
||||
btScalar newAccImpulse = m_accMotorImpulse + unclippedMotorImpulse;
|
||||
btScalar clippedMotorImpulse = unclippedMotorImpulse;
|
||||
if (newAccImpulse > fMaxImpulse)
|
||||
{
|
||||
newAccImpulse = fMaxImpulse;
|
||||
clippedMotorImpulse = newAccImpulse - m_accMotorImpulse;
|
||||
}
|
||||
else if (newAccImpulse < -fMaxImpulse)
|
||||
{
|
||||
newAccImpulse = -fMaxImpulse;
|
||||
clippedMotorImpulse = newAccImpulse - m_accMotorImpulse;
|
||||
}
|
||||
m_accMotorImpulse += clippedMotorImpulse;
|
||||
|
||||
bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,clippedMotorImpulse);
|
||||
bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-clippedMotorImpulse);
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -637,12 +665,16 @@ void btHingeConstraint::updateRHS(btScalar timeStep)
|
||||
}
|
||||
|
||||
|
||||
|
||||
btScalar btHingeConstraint::getHingeAngle()
|
||||
{
|
||||
const btVector3 refAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0);
|
||||
const btVector3 refAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1);
|
||||
const btVector3 swingAxis = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(1);
|
||||
return getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
|
||||
}
|
||||
|
||||
btScalar btHingeConstraint::getHingeAngle(const btTransform& transA,const btTransform& transB)
|
||||
{
|
||||
const btVector3 refAxis0 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
|
||||
const btVector3 refAxis1 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1);
|
||||
const btVector3 swingAxis = transB.getBasis() * m_rbBFrame.getBasis().getColumn(1);
|
||||
btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
|
||||
return m_referenceSign * angle;
|
||||
}
|
||||
@ -676,10 +708,10 @@ void btHingeConstraint::testLimit()
|
||||
#else
|
||||
|
||||
|
||||
void btHingeConstraint::testLimit()
|
||||
void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& transB)
|
||||
{
|
||||
// Compute limit information
|
||||
m_hingeAngle = getHingeAngle();
|
||||
m_hingeAngle = getHingeAngle(transA,transB);
|
||||
m_correction = btScalar(0.);
|
||||
m_limitSign = btScalar(0.);
|
||||
m_solveLimit = false;
|
||||
@ -741,7 +773,7 @@ void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt)
|
||||
}
|
||||
|
||||
// compute angular velocity
|
||||
btScalar curAngle = getHingeAngle();
|
||||
btScalar curAngle = getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
|
||||
btScalar dAngle = targetAngle - curAngle;
|
||||
m_motorTargetVelocity = dAngle / dt;
|
||||
}
|
||||
|
||||
@ -26,7 +26,7 @@ class btRigidBody;
|
||||
|
||||
/// hinge constraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
|
||||
/// axis defines the orientation of the hinge axis
|
||||
class btHingeConstraint : public btTypedConstraint
|
||||
ATTRIBUTE_ALIGNED16(class) btHingeConstraint : public btTypedConstraint
|
||||
{
|
||||
#ifdef IN_PARALLELL_SOLVER
|
||||
public:
|
||||
@ -81,8 +81,12 @@ public:
|
||||
|
||||
virtual void getInfo1 (btConstraintInfo1* info);
|
||||
|
||||
void getInfo1NonVirtual(btConstraintInfo1* info);
|
||||
|
||||
virtual void getInfo2 (btConstraintInfo2* info);
|
||||
|
||||
|
||||
void getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
|
||||
|
||||
virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep);
|
||||
|
||||
void updateRHS(btScalar timeStep);
|
||||
@ -174,7 +178,9 @@ public:
|
||||
|
||||
btScalar getHingeAngle();
|
||||
|
||||
void testLimit();
|
||||
btScalar getHingeAngle(const btTransform& transA,const btTransform& transB);
|
||||
|
||||
void testLimit(const btTransform& transA,const btTransform& transB);
|
||||
|
||||
|
||||
const btTransform& getAFrame() const { return m_rbAFrame; };
|
||||
|
||||
@ -28,7 +28,7 @@ subject to the following restrictions:
|
||||
/// Jacobian entry is an abstraction that allows to describe constraints
|
||||
/// it can be used in combination with a constraint solver
|
||||
/// Can be used to relate the effect of an impulse to the constraint error
|
||||
class btJacobianEntry
|
||||
ATTRIBUTE_ALIGNED16(class) btJacobianEntry
|
||||
{
|
||||
public:
|
||||
btJacobianEntry() {};
|
||||
|
||||
@ -43,6 +43,7 @@ m_useSolveConstraintObsolete(false)
|
||||
|
||||
void btPoint2PointConstraint::buildJacobian()
|
||||
{
|
||||
|
||||
///we need it for both methods
|
||||
{
|
||||
m_appliedImpulse = btScalar(0.);
|
||||
@ -66,10 +67,15 @@ void btPoint2PointConstraint::buildJacobian()
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
void btPoint2PointConstraint::getInfo1 (btConstraintInfo1* info)
|
||||
{
|
||||
getInfo1NonVirtual(info);
|
||||
}
|
||||
|
||||
void btPoint2PointConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
|
||||
{
|
||||
if (m_useSolveConstraintObsolete)
|
||||
{
|
||||
@ -82,22 +88,26 @@ void btPoint2PointConstraint::getInfo1 (btConstraintInfo1* info)
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* info)
|
||||
{
|
||||
getInfo2NonVirtual(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
|
||||
}
|
||||
|
||||
void btPoint2PointConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans)
|
||||
{
|
||||
btAssert(!m_useSolveConstraintObsolete);
|
||||
|
||||
//retrieve matrices
|
||||
btTransform body0_trans;
|
||||
body0_trans = m_rbA.getCenterOfMassTransform();
|
||||
btTransform body1_trans;
|
||||
body1_trans = m_rbB.getCenterOfMassTransform();
|
||||
|
||||
// anchor points in global coordinates with respect to body PORs.
|
||||
|
||||
// set jacobian
|
||||
info->m_J1linearAxis[0] = 1;
|
||||
info->m_J1linearAxis[info->rowskip+1] = 1;
|
||||
info->m_J1linearAxis[2*info->rowskip+2] = 1;
|
||||
info->m_J1linearAxis[info->rowskip+1] = 1;
|
||||
info->m_J1linearAxis[2*info->rowskip+2] = 1;
|
||||
|
||||
btVector3 a1 = body0_trans.getBasis()*getPivotInA();
|
||||
{
|
||||
@ -150,6 +160,7 @@ void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* info)
|
||||
|
||||
void btPoint2PointConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
|
||||
{
|
||||
|
||||
if (m_useSolveConstraintObsolete)
|
||||
{
|
||||
btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
|
||||
@ -220,6 +231,7 @@ void btPoint2PointConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolv
|
||||
normal[i] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void btPoint2PointConstraint::updateRHS(btScalar timeStep)
|
||||
|
||||
@ -36,7 +36,7 @@ struct btConstraintSetting
|
||||
};
|
||||
|
||||
/// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space
|
||||
class btPoint2PointConstraint : public btTypedConstraint
|
||||
ATTRIBUTE_ALIGNED16(class) btPoint2PointConstraint : public btTypedConstraint
|
||||
{
|
||||
#ifdef IN_PARALLELL_SOLVER
|
||||
public:
|
||||
@ -65,8 +65,11 @@ public:
|
||||
|
||||
virtual void getInfo1 (btConstraintInfo1* info);
|
||||
|
||||
void getInfo1NonVirtual (btConstraintInfo1* info);
|
||||
|
||||
virtual void getInfo2 (btConstraintInfo2* info);
|
||||
|
||||
void getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans);
|
||||
|
||||
virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep);
|
||||
|
||||
|
||||
@ -22,7 +22,7 @@ class btIDebugDraw;
|
||||
#include "btSolverBody.h"
|
||||
#include "btSolverConstraint.h"
|
||||
#include "btTypedConstraint.h"
|
||||
|
||||
#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
|
||||
|
||||
///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method.
|
||||
class btSequentialImpulseConstraintSolver : public btConstraintSolver
|
||||
|
||||
@ -126,6 +126,7 @@ void btSliderConstraint::buildJacobian()
|
||||
|
||||
void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB)
|
||||
{
|
||||
#ifndef __SPU__
|
||||
//calculate transforms
|
||||
m_calculatedTransformA = rbA.getCenterOfMassTransform() * frameInA;
|
||||
m_calculatedTransformB = rbB.getCenterOfMassTransform() * frameInB;
|
||||
@ -175,10 +176,10 @@ void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, co
|
||||
// clear accumulator for motors
|
||||
m_accumulatedLinMotorImpulse = btScalar(0.0);
|
||||
m_accumulatedAngMotorImpulse = btScalar(0.0);
|
||||
#endif //__SPU__
|
||||
}
|
||||
|
||||
|
||||
|
||||
void btSliderConstraint::getInfo1(btConstraintInfo1* info)
|
||||
{
|
||||
if (m_useSolveConstraintObsolete)
|
||||
@ -191,7 +192,7 @@ void btSliderConstraint::getInfo1(btConstraintInfo1* info)
|
||||
info->m_numConstraintRows = 4; // Fixed 2 linear + 2 angular
|
||||
info->nub = 2;
|
||||
//prepare constraint
|
||||
calculateTransforms();
|
||||
calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
|
||||
testLinLimits();
|
||||
if(getSolveLinLimit() || getPoweredLinMotor())
|
||||
{
|
||||
@ -207,14 +208,34 @@ void btSliderConstraint::getInfo1(btConstraintInfo1* info)
|
||||
}
|
||||
}
|
||||
|
||||
void btSliderConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
|
||||
{
|
||||
|
||||
info->m_numConstraintRows = 6; // Fixed 2 linear + 2 angular + 1 limit (even if not used)
|
||||
info->nub = 0;
|
||||
}
|
||||
|
||||
void btSliderConstraint::getInfo2(btConstraintInfo2* info)
|
||||
{
|
||||
btAssert(!m_useSolveConstraintObsolete);
|
||||
int i, s = info->rowskip;
|
||||
const btTransform& trA = getCalculatedTransformA();
|
||||
const btTransform& trB = getCalculatedTransformB();
|
||||
|
||||
getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(), m_rbA.getLinearVelocity(),m_rbB.getLinearVelocity(), m_rbA.getInvMass(),m_rbB.getInvMass());
|
||||
}
|
||||
|
||||
void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB, const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass )
|
||||
{
|
||||
//prepare constraint
|
||||
calculateTransforms(transA,transB);
|
||||
testLinLimits();
|
||||
testAngLimits();
|
||||
|
||||
const btTransform& trA = getCalculatedTransformA();
|
||||
const btTransform& trB = getCalculatedTransformB();
|
||||
|
||||
btAssert(!m_useSolveConstraintObsolete);
|
||||
int i, s = info->rowskip;
|
||||
|
||||
btScalar signFact = m_useLinearReferenceFrameA ? btScalar(1.0f) : btScalar(-1.0f);
|
||||
// make rotations around Y and Z equal
|
||||
// the slider axis should be the only unconstrained
|
||||
@ -269,12 +290,12 @@ void btSliderConstraint::getInfo2(btConstraintInfo2* info)
|
||||
// result in three equations, so we project along the planespace vectors
|
||||
// so that sliding along the slider axis is disregarded. for symmetry we
|
||||
// also consider rotation around center of mass of two bodies (factA and factB).
|
||||
btTransform bodyA_trans = m_rbA.getCenterOfMassTransform();
|
||||
btTransform bodyB_trans = m_rbB.getCenterOfMassTransform();
|
||||
btTransform bodyA_trans = transA;
|
||||
btTransform bodyB_trans = transB;
|
||||
int s2 = 2 * s, s3 = 3 * s;
|
||||
btVector3 c;
|
||||
btScalar miA = m_rbA.getInvMass();
|
||||
btScalar miB = m_rbB.getInvMass();
|
||||
btScalar miA = rbAinvMass;
|
||||
btScalar miB = rbBinvMass;
|
||||
btScalar miS = miA + miB;
|
||||
btScalar factA, factB;
|
||||
if(miS > btScalar(0.f))
|
||||
@ -389,8 +410,8 @@ void btSliderConstraint::getInfo2(btConstraintInfo2* info)
|
||||
btScalar bounce = btFabs(btScalar(1.0) - getDampingLimLin());
|
||||
if(bounce > btScalar(0.0))
|
||||
{
|
||||
btScalar vel = m_rbA.getLinearVelocity().dot(ax1);
|
||||
vel -= m_rbB.getLinearVelocity().dot(ax1);
|
||||
btScalar vel = linVelA.dot(ax1);
|
||||
vel -= linVelB.dot(ax1);
|
||||
vel *= signFact;
|
||||
// only apply bounce if the velocity is incoming, and if the
|
||||
// resulting c[] exceeds what we already have.
|
||||
@ -539,6 +560,7 @@ void btSliderConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBod
|
||||
|
||||
void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB)
|
||||
{
|
||||
#ifndef __SPU__
|
||||
int i;
|
||||
// linear
|
||||
btVector3 velA;
|
||||
@ -719,22 +741,24 @@ void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btSolverBody& body
|
||||
bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-angImpulse);
|
||||
}
|
||||
}
|
||||
#endif //__SPU__
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void btSliderConstraint::calculateTransforms(void){
|
||||
void btSliderConstraint::calculateTransforms(const btTransform& transA,const btTransform& transB)
|
||||
{
|
||||
if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete))
|
||||
{
|
||||
m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
|
||||
m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB;
|
||||
m_calculatedTransformA = transA * m_frameInA;
|
||||
m_calculatedTransformB = transB * m_frameInB;
|
||||
}
|
||||
else
|
||||
{
|
||||
m_calculatedTransformA = m_rbB.getCenterOfMassTransform() * m_frameInB;
|
||||
m_calculatedTransformB = m_rbA.getCenterOfMassTransform() * m_frameInA;
|
||||
m_calculatedTransformA = transB * m_frameInB;
|
||||
m_calculatedTransformB = transA * m_frameInA;
|
||||
}
|
||||
m_realPivotAInW = m_calculatedTransformA.getOrigin();
|
||||
m_realPivotBInW = m_calculatedTransformB.getOrigin();
|
||||
|
||||
@ -131,9 +131,13 @@ public:
|
||||
// overrides
|
||||
virtual void buildJacobian();
|
||||
virtual void getInfo1 (btConstraintInfo1* info);
|
||||
|
||||
void getInfo1NonVirtual(btConstraintInfo1* info);
|
||||
|
||||
virtual void getInfo2 (btConstraintInfo2* info);
|
||||
|
||||
void getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass);
|
||||
|
||||
virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep);
|
||||
|
||||
|
||||
@ -215,13 +219,13 @@ public:
|
||||
void buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB);
|
||||
void solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB);
|
||||
// shared code used by ODE solver
|
||||
void calculateTransforms(void);
|
||||
void testLinLimits(void);
|
||||
void calculateTransforms(const btTransform& transA,const btTransform& transB);
|
||||
void testLinLimits();
|
||||
void testLinLimits2(btConstraintInfo2* info);
|
||||
void testAngLimits(void);
|
||||
void testAngLimits();
|
||||
// access for PE Solver
|
||||
btVector3 getAncorInA(void);
|
||||
btVector3 getAncorInB(void);
|
||||
btVector3 getAncorInA();
|
||||
btVector3 getAncorInB();
|
||||
};
|
||||
|
||||
|
||||
|
||||
@ -30,7 +30,8 @@ enum btTypedConstraintType
|
||||
HINGE_CONSTRAINT_TYPE,
|
||||
CONETWIST_CONSTRAINT_TYPE,
|
||||
D6_CONSTRAINT_TYPE,
|
||||
SLIDER_CONSTRAINT_TYPE
|
||||
SLIDER_CONSTRAINT_TYPE,
|
||||
CONTACT_CONSTRAINT_TYPE
|
||||
};
|
||||
|
||||
///TypedConstraint is the baseclass for Bullet constraints and vehicles
|
||||
|
||||
@ -81,6 +81,11 @@ public:
|
||||
|
||||
virtual void setNumTasks(int numTasks);
|
||||
|
||||
virtual int getNumTasks() const
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif //SEQUENTIAL_THREAD_SUPPORT_H
|
||||
|
||||
@ -183,6 +183,8 @@ void Win32ThreadSupport::startThreads(const Win32ThreadConstructionInfo& threadC
|
||||
m_activeSpuStatus.resize(threadConstructionInfo.m_numThreads);
|
||||
m_completeHandles.resize(threadConstructionInfo.m_numThreads);
|
||||
|
||||
m_maxNumTasks = threadConstructionInfo.m_numThreads;
|
||||
|
||||
for (int i=0;i<threadConstructionInfo.m_numThreads;i++)
|
||||
{
|
||||
printf("starting thread %d\n",i);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user