Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Minor constraint refactoring, to allow SPU-side processing for PLAYSTATION 3 (added non-virtual methods)

Browse Source 
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
2009-08-05 22:14:46 +00:00
parent b16f251530
commit 3e2529fcb5
21 changed files with 2980 additions and 2587 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
Extras/readblend/abs-file.h
View File

@@ -10,13 +10,18 @@
#ifndef _ABS_FILE_H #ifndef _ABS_FILE_H
#define _ABS_FILE_H #define _ABS_FILE_H
#ifdef WIN32
typedef unsigned char uint8_t; typedef unsigned char uint8_t;
typedef unsigned long int uint64_t; typedef unsigned long int uint64_t;
typedef unsigned int uint32_t; typedef unsigned int uint32_t;
typedef int int32_t; typedef int int32_t;
typedef unsigned short uint16_t; typedef unsigned short uint16_t;
typedef short int16_t; typedef short int16_t;
#else
#include <stdint.h>
#endif
/* /*
PLEASE NOTE: This license applies to abs-file.h ONLY; The version of 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 */ #else /* !USE_PHYSFS */
//#define USE_POSIX_FILES 1
#ifdef USE_POSIX_FILES
#define MY_FILETYPE FILE #define MY_FILETYPE FILE
#define MY_READ(p,s,n,fp) fread(p,s,n,fp) #define MY_READ(p,s,n,fp) fread(p,s,n,fp)
#define MY_WRITE(p,s,n,fp) fwrite(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 */ fseek(fp, currentpos, SEEK_SET); /* restore previous cursor position */
return newpos; 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 /* USE_PHYSFS */
#endif /* _ABS_FILE_H */ #endif /* _ABS_FILE_H */

87
Extras/readblend/blendtype.h Normal file
View 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

658
Extras/readblend/readblend.c
View File

@@ -1,3 +1,4 @@
#include "blendtype.h"
#include <stdlib.h> #include <stdlib.h>
//#include <stdint.h> //#include <stdint.h>
@@ -6,6 +7,90 @@
#include "abs-file.h" #include "abs-file.h"
#ifndef USE_POSIX_FILES
static char* gBuffer = 0;
static int gCurrentFilePtr = 0;
static int gFileSize = 0;
long MY_FILELENGTH(FILE *fp) {
long currentpos = ftell(fp); /* save current cursor position */
long newpos;
fseek(fp, 0, SEEK_END); /* seek to end */
newpos = ftell(fp); /* find position of end -- this is the length */
fseek(fp, currentpos, SEEK_SET); /* restore previous cursor position */
return newpos;
}
MY_FILETYPE* MY_OPEN_FOR_READ(const char *const filename)
{
FILE* gFile = fopen(filename,"rb");
if (gFile)
{
long currentpos = ftell(gFile); /* save current cursor position */
long newpos;
int bytesRead;
fseek(gFile, 0, SEEK_END); /* seek to end */
newpos = ftell(gFile); /* find position of end -- this is the length */
fseek(gFile, currentpos, SEEK_SET); /* restore previous cursor position */
gFileSize = newpos;
gBuffer = (char*)malloc(gFileSize);
bytesRead = fread(gBuffer,gFileSize,1,gFile);
gCurrentFilePtr = 0;
fclose(gFile);
gFile = 0;
return gBuffer;
}
return 0;
}
int MY_GETC(MY_FILETYPE *const fp)
{
return gBuffer[gCurrentFilePtr++];
}
void MY_SEEK(MY_FILETYPE* fp,int pos)
{
gCurrentFilePtr = pos;
}
#define MY_REWIND(fp) MY_SEEK(fp,0)
int MY_READ(unsigned char* dest,int size,int num,MY_FILETYPE* fp)
{
int n;
memcpy(dest,&gBuffer[gCurrentFilePtr],size*num);
gCurrentFilePtr += size*num;
return num;
}
void MY_CLOSE(MY_FILETYPE* fp)
{
free(gBuffer);
gBuffer = 0;
gCurrentFilePtr = 0;
gFileSize = 0;
}
int MY_TELL(MY_FILETYPE* fp)
{
return gCurrentFilePtr;
}
int MY_ATEOF(MY_FILETYPE* fp)
{
return gCurrentFilePtr<gFileSize? 0 : 1;
}
#endif //USE_POSIX_FILES
#define B_DEBUG #define B_DEBUG
@@ -19,77 +104,19 @@
# define dprintf # define dprintf
#endif #endif
int tryit = 0;
/* the types extracted from the Blender file */ int dumpNames= 1;
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 */
};
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;
};
typedef struct _BlendBlock BlendBlock;
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;
};
/* the opaque BlendFile structure */
typedef struct {
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 */
} IBlendFile;
#define BlendFile IBlendFile
#include "readblend.h" #include "readblend.h"
#define MAX_CACHED_IMAGES 1024
BlendBlockPointer tCachedTPage[MAX_CACHED_IMAGES];
bImage* tCachedImages[MAX_CACHED_IMAGES];
int gNumCachedImages=0;
/* endianness conversion macros */ /* endianness conversion macros */
#define BGETLEUINT16(p) ((uint16_t) ( \ #define BGETLEUINT16(p) ((uint16_t) ( \
( ((uint16_t)*( 1 + (uint8_t*)(p) ))<<8 ) | \ ( ((uint16_t)*( 1 + (uint8_t*)(p) ))<<8 ) | \
@@ -176,24 +203,24 @@ seek_past_string(MY_FILETYPE* file, const char *str) {
} }
#define EXPANDO_MULTIPLE(ARRAY,ADDITIONPTR,NUMBER) \ #define EXPANDO_MULTIPLE(ARRAY,ADDITIONPTR,NUMBER) \
do { \ do { \
(ARRAY) = realloc((ARRAY), sizeof((ARRAY)[0]) * (NUMBER +(ARRAY##_count))); \ (ARRAY) = realloc((ARRAY), sizeof((ARRAY)[0]) * (NUMBER +(ARRAY##_count))); \
memcpy(&(ARRAY)[ARRAY##_count], ADDITIONPTR, sizeof((ARRAY)[0]) * NUMBER); \ memcpy(&(ARRAY)[ARRAY##_count], ADDITIONPTR, sizeof((ARRAY)[0]) * NUMBER); \
(ARRAY##_count) += NUMBER; \ (ARRAY##_count) += NUMBER; \
} while(0) } while(0)
#define EXPANDO(ARRAY,ADDITION) \ #define EXPANDO(ARRAY,ADDITION) \
do { \ do { \
(ARRAY) = realloc((ARRAY), sizeof((ARRAY)[0]) * (1 + (ARRAY##_count))); \ (ARRAY) = realloc((ARRAY), sizeof((ARRAY)[0]) * (1 + (ARRAY##_count))); \
(ARRAY)[ARRAY##_count] = (ADDITION); \ (ARRAY)[ARRAY##_count] = (ADDITION); \
++(ARRAY##_count); \ ++(ARRAY##_count); \
} while(0) } while(0)
static unsigned short static unsigned short
read_ushort(MY_FILETYPE* file) { read_ushort(MY_FILETYPE* file) {
unsigned char c[2]; unsigned char c[2];
if (MY_READ(c, 2, 1, file) == 1) { if (MY_READ(c, 2, 1, file) == 1)
{
return c[0] | (c[1]<<8); return c[0] | (c[1]<<8);
} else { } else {
return 0xFFFF; return 0xFFFF;
@@ -240,7 +267,7 @@ name_is_pointer(char* name) {
/* note: we only deal with 1d or 2d arrays at the moment. haven't /* note: we only deal with 1d or 2d arrays at the moment. haven't
seen any arrays of a higher order from Blender yet. */ seen any arrays of a higher order from Blender yet. */
static int static int
name_is_array(char* name, int* dim1, int* dim2) { name_is_array(char* name, int* dim1, int* dim2) {
int len = strlen(name); int len = strlen(name);
@@ -352,8 +379,20 @@ recursively_read_type(MY_FILETYPE* file, BlendFile* bf,
new_data[1] = MY_GETC(file); new_data[1] = MY_GETC(file);
new_data[2] = MY_GETC(file); new_data[2] = MY_GETC(file);
new_data[3] = MY_GETC(file); new_data[3] = MY_GETC(file);
EXPANDO(field->field_offsets, field->field_bytes_count); //EXPANDO(field->field_offsets, field->field_bytes_count);
EXPANDO_MULTIPLE(field->field_bytes, new_data, new_data_size);
(field->field_offsets) = realloc((field->field_offsets), sizeof((field->field_offsets)[0]) * (1 + (field->field_offsets_count)));
(field->field_offsets)[field->field_offsets_count] = (field->field_bytes_count);
++(field->field_offsets_count);
// EXPANDO_MULTIPLE(field->field_bytes, new_data, new_data_size);
{
(field->field_bytes) = realloc((field->field_bytes), sizeof((field->field_bytes)[0]) * (new_data_size +(field->field_bytes_count)));
memcpy(&(field->field_bytes)[field->field_bytes_count], new_data, sizeof((field->field_bytes)[0]) * new_data_size);
(field->field_bytes_count) += new_data_size;
}
/*fprintf(stderr, "N*(%d) ", new_data_size);*/ /*fprintf(stderr, "N*(%d) ", new_data_size);*/
} }
} }
@@ -426,21 +465,41 @@ blend_read_data(MY_FILETYPE* file, BlendFile* bf)
unsigned long section_pointer; unsigned long section_pointer;
unsigned long section_type; unsigned long section_type;
unsigned long section_ents; unsigned long section_ents;
MY_SEEK(file, next_block_start); MY_SEEK(file, next_block_start);
MY_READ(section_name, 4, 1, file); MY_READ(section_name, 4, 1, file);
if (strcmp(section_name, "DNA1") != 0) { if (strcmp(section_name, "DNA1") != 0) {
int i; int i;
BlendBlock block; BlendBlock block;
if (strcmp(section_name, "IM") == 0) {
//printf("image\n");
}
if (strcmp(section_name, "IP") == 0) {
// printf("ipo\n");
}
section_size = read_ulong(file); section_size = read_ulong(file);
section_pointer = read_ulong(file); section_pointer = read_ulong(file);
section_type = read_ulong(file); section_type = read_ulong(file);
section_type = bf->strc_indices[section_type]; section_type = bf->strc_indices[section_type];
section_ents = read_ulong(file); section_ents = read_ulong(file);
if (dumpNames)
{
// printf("section_name = %s, section_type = %d / %s\n",section_name,section_type,bf->types[section_type]);
}
memcpy(block.tag, section_name, 4); memcpy(block.tag, section_name, 4);
block.tag[4] = '\0'; block.tag[4] = '\0';
block.type_index = section_type; block.type_index = section_type;
@@ -448,14 +507,28 @@ blend_read_data(MY_FILETYPE* file, BlendFile* bf)
/*block.fixed_pointer = NULL;*/ /*block.fixed_pointer = NULL;*/
block.array_entries = NULL; block.array_entries = NULL;
block.array_entries_count = 0; block.array_entries_count = 0;
block.customData = 0;
block.customDataSize = 0;
/*dprintf(stderr, "\nsizeof(%s)=%ld: %s[%ld]\n", section_name, section_size, bf->types[section_type].name, section_ents); */ // dprintf(stderr, "\nsizeof(%s)=%ld: %s[%ld]\n", section_name, section_size, bf->types[section_type].name, section_ents);
if (strcmp("Link",bf->types[section_type].name)==0)
{
if (section_size>0)
{
//read customData
block.customData= (char*)malloc(section_size);
block.customDataSize = section_size;
MY_READ(block.customData,1,section_size,file);
}
} else
{
for (i=0; i<section_ents; ++i) { for (i=0; i<section_ents; ++i) {
BlendField field; BlendField field;
field = read_type(file, bf, section_type); field = read_type(file, bf, section_type);
EXPANDO(block.array_entries, field); EXPANDO(block.array_entries, field);
} }
}
EXPANDO(bf->blocks, block); EXPANDO(bf->blocks, block);
@@ -463,15 +536,14 @@ blend_read_data(MY_FILETYPE* file, BlendFile* bf)
#ifdef B_DEBUG #ifdef B_DEBUG
if (MY_TELL(file) > next_block_start) { if (MY_TELL(file) > next_block_start) {
dprintf(stderr, " **OVER-READ(%ld,%ld)** ", //dprintf(stderr, " **OVER-READ(%ld,%ld)** ", MY_TELL(file), next_block_start);
MY_TELL(file), next_block_start);
if (strcmp(bf->types[section_type].name, "Link") == 0 && if (strcmp(bf->types[section_type].name, "Link") == 0 &&
MY_TELL(file) - next_block_start == 4) { MY_TELL(file) - next_block_start == 4) {
dprintf(stderr, "<- don't panic, known Link struct weirdness."); //dprintf(stderr, "<- don't panic, known Link struct weirdness.");
} else { } else {
dprintf(stderr, "<- okay, PANIC!"); dprintf(stderr, "<- okay, PANIC!");
} }
dprintf(stderr, "\n"); //dprintf(stderr, "\n");
} else if (MY_TELL(file) < next_block_start) { } else if (MY_TELL(file) < next_block_start) {
/*dprintf(stderr, " **under-READ(%ld,%ld)** ", /*dprintf(stderr, " **under-READ(%ld,%ld)** ",
MY_TELL(file), next_block_start);*/ MY_TELL(file), next_block_start);*/
@@ -493,7 +565,7 @@ blend_read_data(MY_FILETYPE* file, BlendFile* bf)
BlendFile* BlendFile*
blend_read(MY_FILETYPE* file) blend_read(MY_FILETYPE* file)
{ {
char blender_mark[8] = {0,0,0,0, 0,0,0,0}; char blender_mark[10] = {0,0,0,0, 0,0,0,0,0,0};
BlendFile *bf; BlendFile *bf;
long sdnaname_offs, type_offs, tlen_offs, strc_offs, endb_offs; long sdnaname_offs, type_offs, tlen_offs, strc_offs, endb_offs;
long sdnaname_size, type_size, tlen_size, strc_size; long sdnaname_size, type_size, tlen_size, strc_size;
@@ -503,9 +575,9 @@ blend_read(MY_FILETYPE* file)
/* Check file signature */ /* Check file signature */
MY_READ(blender_mark, 1, 7, file); MY_READ(blender_mark, 1, 9, file);
if (strcmp(blender_mark, "BLENDER") != 0) { if (strcmp(blender_mark, "BLENDER_v") != 0) {
dprintf(stderr, "Not a Blender file.\n"); dprintf(stderr, "Not a valid Blender file (.blend file needs to be written on a 32bit little-endian machine)\n");
return NULL; return NULL;
} }
@@ -742,14 +814,14 @@ blend_free(BlendFile* blend_file)
/******************************************************************** /********************************************************************
* done with the reading/parsing logic; now for the querying logic * * done with the reading/parsing logic; now for the querying logic *
********************************************************************/ ********************************************************************/
/******************************************************************** /********************************************************************
* LOW-LEVEL * * LOW-LEVEL *
********************************************************************/ ********************************************************************/
@@ -818,7 +890,8 @@ blend_type_basename_compare(const char *fancy, const char *raw) {
static BlendObjType static BlendObjType
typestring_to_blendobj_type(BlendFile* blend_file, typestring_to_blendobj_type(BlendFile* blend_file,
const char* type_name) { const char* type_name)
{
if (blend_type_basename_compare(type_name, "char") == 0) { if (blend_type_basename_compare(type_name, "char") == 0) {
return BLEND_OBJ_CHAR8; return BLEND_OBJ_CHAR8;
} else if (blend_type_basename_compare(type_name, "uchar") == 0) { } else if (blend_type_basename_compare(type_name, "uchar") == 0) {
@@ -908,6 +981,9 @@ blend_object_array_getdata(BlendFile* blend_file,
void* data; void* data;
int dim1, dim2; int dim1, dim2;
if (!bf)
return 0;
name_is_array(blend_file->names[obj.name], &dim1, &dim2); name_is_array(blend_file->names[obj.name], &dim1, &dim2);
/*dprintf(stderr, "copying:'%s'[%d][%d] (of [%d][%d]) ", type_name, dim_index_1, dim_index_2, dim1, dim2);*/ /*dprintf(stderr, "copying:'%s'[%d][%d] (of [%d][%d]) ", type_name, dim_index_1, dim_index_2, dim1, dim2);*/
@@ -985,7 +1061,7 @@ blend_object_getdata(BlendFile* blend_file,
/* recursively count the number of fields and array items in this /* recursively count the number of fields and array items in this
structure, for the purposes of skipping in the field offset array */ structure, for the purposes of skipping in the field offset array */
static long static long
get_num_type_segments(BlendFile* blend_file, get_num_type_segments(BlendFile* blend_file,
BlendObject obj) BlendObject obj)
@@ -1091,8 +1167,8 @@ blend_block_get_object(BlendFile* blend_file,
/******************************************************************** /********************************************************************
* MID-LEVEL * * MID-LEVEL *
********************************************************************/ ********************************************************************/
/* general helpers */ /* general helpers */
@@ -1135,7 +1211,7 @@ blend_object_getstring(BlendFile* blend_file,
dest[strpos] = c; dest[strpos] = c;
} }
done: done:
dest[strpos] = '\0'; dest[strpos] = '\0';
return rtn; return rtn;
} }
@@ -1146,7 +1222,8 @@ blend_object_getstring(BlendFile* blend_file,
int int
blend_object_get_IDname(BlendFile* blend_file, blend_object_get_IDname(BlendFile* blend_file,
BlendObject obj, BlendObject obj,
char *dest, int max_chars) { char *dest, int max_chars)
{
if (BLEND_OBJ_STRUCT == blend_object_type(blend_file, obj)) { if (BLEND_OBJ_STRUCT == blend_object_type(blend_file, obj)) {
BlendObject idstruc_obj; BlendObject idstruc_obj;
/* See if this structure has an 'id' field */ /* See if this structure has an 'id' field */
@@ -1214,7 +1291,7 @@ block_ID_finder(BLENDBLOCKCALLBACK_ARGS) {
} }
} }
done:; done:;
free(obj_string); free(obj_string);
} }
@@ -1265,12 +1342,13 @@ blend_dump_typedefs(BlendFile* bf)
bf->types[bf->types[i].fieldtypes[j]].name, bf->types[bf->types[i].fieldtypes[j]].name,
bf->names[bf->types[i].fieldnames[j]]); bf->names[bf->types[i].fieldnames[j]]);
if (j == bf->types[i].fieldtypes_count-1) { if (j == bf->types[i].fieldtypes_count-1) {
printf("; }\n"); printf(";}");
} else { } else {
printf("; "); printf("; ");
} }
} }
} }
printf("\n\n");
} }
} }
@@ -1324,9 +1402,9 @@ blend_obj_get_layermask(BlendFile *bf, BlendObject *objobj) {
/* Bizarrely, blender doesn't seem to carry a mapping of parent /* Bizarrely, blender doesn't seem to carry a mapping of parent
to children -- only of children to parent. So, to find the children to children -- only of children to parent. So, to find the children
of an Object we have to scan all objects to see if their parent is of an Object we have to scan all objects to see if their parent is
the Object in question. */ the Object in question. */
typedef struct { typedef struct {
BlendBlockPointer wanted_parent; BlendBlockPointer wanted_parent;
BlendBlockPointer child; BlendBlockPointer child;
@@ -1394,8 +1472,8 @@ blend_obj_get_child(BlendFile *bf, BlendObject *objobj, int childnum) {
/******************************************************************** /********************************************************************
* HIGH-LEVEL * * HIGH-LEVEL *
********************************************************************/ ********************************************************************/
#ifndef LEN3POW2 #ifndef LEN3POW2
/* length squared */ /* length squared */
@@ -1410,15 +1488,15 @@ blend_obj_get_child(BlendFile *bf, BlendObject *objobj, int childnum) {
#ifndef NORMALIZE3 #ifndef NORMALIZE3
/* vector normalization (expensive) */ /* vector normalization (expensive) */
#define NORMALIZE3(xd,yd,zd) \ #define NORMALIZE3(xd,yd,zd) \
do { \ do { \
const double norm3_macro_len3 = LEN3((xd),(yd),(zd)); \ const double norm3_macro_len3 = LEN3((xd),(yd),(zd)); \
if (norm3_macro_len3 != 0.0F) \ if (norm3_macro_len3 != 0.0F) \
{ \ { \
(xd) = (xd) / norm3_macro_len3; \ (xd) = (xd) / norm3_macro_len3; \
(yd) = (yd) / norm3_macro_len3; \ (yd) = (yd) / norm3_macro_len3; \
(zd) = (zd) / norm3_macro_len3; \ (zd) = (zd) / norm3_macro_len3; \
} \ } \
} while (0) } while (0)
#endif #endif
@@ -1533,11 +1611,13 @@ blend_free_obj(bObj *obj) {
void void
blend_acquire_obj_from_obj(BlendFile *bf, BlendObject *objobj, blend_acquire_obj_from_obj(BlendFile *bf, BlendObject *objobj,
bObj *outobj, BlendObject **mallocdoutblendobject) { bObj *outobj, BlendObject **mallocdoutblendobject)
{
BlendObject obj, dataobj; BlendObject obj, dataobj;
BlendBlockPointer block; BlendBlockPointer block,ipoblock,curveblock=0;
short sdata = 12345; short sdata = 12345;
#define B_IDNAME_MAX_SIZE 80 #define B_IDNAME_MAX_SIZE 80
char *idname = malloc(1 + B_IDNAME_MAX_SIZE); char *idname = malloc(1 + B_IDNAME_MAX_SIZE);
float fdata1, fdata2, fdata3; float fdata1, fdata2, fdata3;
@@ -1548,14 +1628,17 @@ blend_acquire_obj_from_obj(BlendFile *bf, BlendObject *objobj,
if (!(blend_object_structure_getfield(bf, &obj, *objobj, if (!(blend_object_structure_getfield(bf, &obj, *objobj,
"type") && "type") &&
blend_object_getdata(bf, &sdata, obj))) { blend_object_getdata(bf, &sdata, obj)))
{
abort(); /* couldn't get type */ abort(); /* couldn't get type */
} else { } else {
switch (sdata) { switch (sdata) {
case 1: /* mesh */ case 1: /* mesh */
outobj->type = BOBJ_TYPE_MESH; break; outobj->type = BOBJ_TYPE_MESH; break;
case 10: /* lamp */ case 10: /* lamp */
outobj->type = BOBJ_TYPE_LAMP; break;
case 11: /* camera */ case 11: /* camera */
outobj->type = BOBJ_TYPE_CAMERA; break;
default: default:
outobj->type = BOBJ_TYPE_UNKNOWN; break; outobj->type = BOBJ_TYPE_UNKNOWN; break;
} }
@@ -1571,6 +1654,7 @@ blend_acquire_obj_from_obj(BlendFile *bf, BlendObject *objobj,
/* now we override the mesh transform with the object's. should /* now we override the mesh transform with the object's. should
we merge, instead??? - hm, dunno, don't think so. */ we merge, instead??? - hm, dunno, don't think so. */
if (blend_object_structure_getfield(bf, &obj, *objobj, "loc") && if (blend_object_structure_getfield(bf, &obj, *objobj, "loc") &&
blend_object_array_getdata(bf, &fdata1, obj, 0,0) && blend_object_array_getdata(bf, &fdata1, obj, 0,0) &&
blend_object_array_getdata(bf, &fdata2, obj, 0,1) && blend_object_array_getdata(bf, &fdata2, obj, 0,1) &&
@@ -1619,7 +1703,7 @@ blend_acquire_obj_from_obj(BlendFile *bf, BlendObject *objobj,
} }
} }
} }
#if 0
if (blend_object_structure_getfield(bf, &obj, *objobj, "transflag")) { if (blend_object_structure_getfield(bf, &obj, *objobj, "transflag")) {
char cdata; char cdata;
/* TODO: decode what these flags precisely mean. */ /* TODO: decode what these flags precisely mean. */
@@ -1628,6 +1712,77 @@ blend_acquire_obj_from_obj(BlendFile *bf, BlendObject *objobj,
outobj->transflags = (unsigned char)cdata; outobj->transflags = (unsigned char)cdata;
} }
} }
#endif
outobj->mass = 0.f;
if (blend_object_structure_getfield(bf, &obj, *objobj, "mass"))
{
float mass;
if (blend_object_getdata(bf, &mass, obj))
{
outobj->mass = mass;
}
}
if ((blend_object_structure_getfield(bf, &obj, *objobj,"ipo") && blend_object_getdata(bf, &ipoblock, obj)))
{
if (ipoblock)
{
BlendObject ipo = blend_block_get_object(bf, ipoblock, 0);
#define MAX_CHARS 31
char iponame[MAX_CHARS];
BlendObject obj2,obj3;
blend_object_get_IDname(bf,ipo,iponame,MAX_CHARS-1);
// printf("ipo.ID.name = %s\n",iponame);
if (blend_object_structure_getfield(bf, &obj2, ipo,"curve"))
{
BlendBlock* block = (BlendBlock*)obj2.block;
void** ptrptr = &block->array_entries->field_bytes[block->array_entries->field_offsets[obj2.field_index]];
BlendBlockPointer ptr = *ptrptr;
//ptrptr++; contains the 'last' pointer
if (ptr)
{
BlendBlockPointer curveblockptr = blend_block_from_blendpointer(bf, ptr);
BlendObject curve = blend_block_get_object(bf, curveblockptr, 0);
}
}
}
}
outobj->boundtype = 0;
if (blend_object_structure_getfield(bf, &obj, *objobj, "boundtype"))
{
short int boundtype;
if (blend_object_getdata(bf, &boundtype, obj))
{
outobj->boundtype= boundtype;
}
}
outobj->gameflag = 0;
if (blend_object_structure_getfield(bf, &obj, *objobj, "gameflag"))
{
int gameflag;
if (blend_object_getdata(bf, &gameflag, obj))
{
outobj->gameflag= gameflag;
}
}
if (blend_object_structure_getfield(bf, &obj, *objobj, "obmat")) { if (blend_object_structure_getfield(bf, &obj, *objobj, "obmat")) {
int i,j; int i,j;
@@ -1641,11 +1796,15 @@ blend_acquire_obj_from_obj(BlendFile *bf, BlendObject *objobj,
} }
} }
/* get actual obj here */ /* get actual mesh obj here */
if (! (blend_object_structure_getfield(bf, &obj, *objobj, "data") && if (! (blend_object_structure_getfield(bf, &obj, *objobj, "data") &&
blend_object_getdata(bf, &block, obj))) { blend_object_getdata(bf, &block, obj)))
abort(); {
printf("no mesh\n");
outobj->data.mesh = 0;
return;
//abort();
} }
if (block == NULL) { if (block == NULL) {
@@ -1670,6 +1829,7 @@ blend_acquire_obj_from_obj(BlendFile *bf, BlendObject *objobj,
outobj->data.dummy = NULL; outobj->data.dummy = NULL;
break; break;
} }
} }
@@ -1707,7 +1867,8 @@ blend_free_texlayer(bTexLayer *tl) {
void /* MTex */ void /* MTex */
blend_acquire_texlayer_from_obj(BlendFile *bf, BlendObject *tlobj, blend_acquire_texlayer_from_obj(BlendFile *bf, BlendObject *tlobj,
bTexLayer *tl) { bTexLayer *tl)
{
BlendObject obj; BlendObject obj;
BlendBlockPointer tex_block; BlendBlockPointer tex_block;
short sdata = 12345; short sdata = 12345;
@@ -1862,7 +2023,8 @@ blend_free_material(bMaterial *mat) {
void void
blend_acquire_material_from_obj(BlendFile *bf, BlendObject *matobj, blend_acquire_material_from_obj(BlendFile *bf, BlendObject *matobj,
bMaterial *mat) { bMaterial *mat)
{
BlendObject obj; BlendObject obj;
int i; int i;
int32_t ldata = 123456; int32_t ldata = 123456;
@@ -1930,8 +2092,7 @@ blend_acquire_mesh_from_obj(BlendFile *bf, BlendObject *meobj, bMesh *mesh)
{ {
{ {
BlendObject obj; BlendObject obj;
BlendBlockPointer vblock, fblock, cblock, tblock, BlendBlockPointer vblock, fblock, cblock, ttblock,mtblock,dblock, matlink;
dblock, matlink;
int i; int i;
int32_t ldata = 123456; int32_t ldata = 123456;
float fdata1, fdata2, fdata3; float fdata1, fdata2, fdata3;
@@ -1940,25 +2101,33 @@ blend_acquire_mesh_from_obj(BlendFile *bf, BlendObject *meobj, bMesh *mesh)
if (!(blend_object_structure_getfield(bf, &obj, *meobj, if (!(blend_object_structure_getfield(bf, &obj, *meobj,
"totvert") && "totvert") &&
blend_object_getdata(bf, &ldata, obj))) { blend_object_getdata(bf, &ldata, obj)))
abort(); {
printf("invalid mesh 0x1\n");
meobj->type = BOBJ_TYPE_INVALID_MESH;
return;
} }
mesh->vert_count = ldata; mesh->vert_count = ldata;
if (!(blend_object_structure_getfield(bf, &obj, *meobj, if (!(blend_object_structure_getfield(bf, &obj, *meobj,
"totface") && "totface") &&
blend_object_getdata(bf, &ldata, obj))) { blend_object_getdata(bf, &ldata, obj)))
abort(); {
printf("invalid mesh 0x2\n");
meobj->type = BOBJ_TYPE_INVALID_MESH;
return;
} }
mesh->face_count = ldata; mesh->face_count = ldata;
dprintf(stderr, "%d verts, %d faces...\n", dprintf(stderr, "%d verts, %d faces...\n", mesh->vert_count, mesh->face_count);
mesh->vert_count, mesh->face_count);
if (!(blend_object_structure_getfield(bf, &obj, *meobj, if (!(blend_object_structure_getfield(bf, &obj, *meobj,
"mface") && "mface") &&
blend_object_getdata(bf, &fblock, obj))) { blend_object_getdata(bf, &fblock, obj)))
abort(); {
printf("invalid mesh 0x3\n");
meobj->type = BOBJ_TYPE_INVALID_MESH;
return;
} }
/* null fblock is okay */ /* null fblock is okay */
@@ -1968,6 +2137,8 @@ blend_acquire_mesh_from_obj(BlendFile *bf, BlendObject *meobj, bMesh *mesh)
/* found an indirect material link, follow it */ /* found an indirect material link, follow it */
BlendObject matlinkobj = blend_block_get_object(bf, matlink, 0); BlendObject matlinkobj = blend_block_get_object(bf, matlink, 0);
BlendBlockPointer matblock; BlendBlockPointer matblock;
if (obj.block)
{
if (blend_object_structure_getfield(bf, &obj, matlinkobj, if (blend_object_structure_getfield(bf, &obj, matlinkobj,
"next") && "next") &&
blend_object_getdata(bf, &matblock, obj)) { blend_object_getdata(bf, &matblock, obj)) {
@@ -1979,7 +2150,8 @@ blend_acquire_mesh_from_obj(BlendFile *bf, BlendObject *meobj, bMesh *mesh)
/* um, okay, link went nowhere, leave mesh->material NULL */ /* um, okay, link went nowhere, leave mesh->material NULL */
} }
} else { } else {
abort(); //abort();//might fail?
}
} }
} else { } else {
/* no material -- mesh->material will remain NULL */ /* no material -- mesh->material will remain NULL */
@@ -1994,10 +2166,16 @@ blend_acquire_mesh_from_obj(BlendFile *bf, BlendObject *meobj, bMesh *mesh)
if (!(blend_object_structure_getfield(bf, &obj, *meobj, if (!(blend_object_structure_getfield(bf, &obj, *meobj,
"tface") && "tface") &&
blend_object_getdata(bf, &tblock, obj))) { blend_object_getdata(bf, &ttblock, obj))) {
abort(); abort();
} }
if (!(blend_object_structure_getfield(bf, &obj, *meobj,
"mtface") &&
blend_object_getdata(bf, &mtblock, obj))) {
mtblock=0;
}
if (!(blend_object_structure_getfield(bf, &obj, *meobj, if (!(blend_object_structure_getfield(bf, &obj, *meobj,
"mcol") && "mcol") &&
blend_object_getdata(bf, &cblock, obj))) { blend_object_getdata(bf, &cblock, obj))) {
@@ -2075,6 +2253,7 @@ blend_acquire_mesh_from_obj(BlendFile *bf, BlendObject *meobj, bMesh *mesh)
mesh->face[i].v[2] = mesh->face[i].v[3] = -1; mesh->face[i].v[2] = mesh->face[i].v[3] = -1;
mesh->face[i].mat = -1; mesh->face[i].mat = -1;
mesh->face[i].flags = 0; mesh->face[i].flags = 0;
for (j=0; j<4; ++j) { for (j=0; j<4; ++j) {
for (k=0; k<3; ++k) { for (k=0; k<3; ++k) {
mesh->face[i].rgba[j][k] = 1.0; mesh->face[i].rgba[j][k] = 1.0;
@@ -2082,7 +2261,7 @@ blend_acquire_mesh_from_obj(BlendFile *bf, BlendObject *meobj, bMesh *mesh)
mesh->face[i].rgba[j][3] = 1.0f; mesh->face[i].rgba[j][3] = 1.0f;
mesh->face[i].uv[j][0] = 0.0f; mesh->face[i].uv[j][0] = 0.0f;
mesh->face[i].uv[j][1] = 0.0f; mesh->face[i].uv[j][1] = 0.0f;
mesh->face[i].image_id = NULL; mesh->face[i].m_image = NULL;
} }
if (blend_object_structure_getfield(bf, &aobj, obj, "v1")) { if (blend_object_structure_getfield(bf, &aobj, obj, "v1")) {
@@ -2212,13 +2391,223 @@ blend_acquire_mesh_from_obj(BlendFile *bf, BlendObject *meobj, bMesh *mesh)
} }
} }
if (tblock) { if (mtblock)
{
/* we have tex co-ords */
for (i=0; i<mesh->face_count; ++i)
{
int j,k;
void *pdata;
BlendObject aobj;
BlendObject obj = blend_block_get_object(bf, mtblock, i);
BlendObject obj2,obj3;
unsigned char flag;
unsigned short int mode;
BlendBlockPointer tpageptr;
if (blend_object_structure_getfield(bf, &obj3, obj, "flag"))
{
blend_object_getdata(bf, &flag, obj3);
mesh->face[i].m_flag = flag;
} else
{
mesh->face[i].m_flag = 0;
}
if (blend_object_structure_getfield(bf, &obj3, obj, "mode"))
{
blend_object_getdata(bf, &mode, obj3);
mesh->face[i].m_mode = mode;
} else
{
mesh->face[i].m_mode = 0;
}
if ((blend_object_structure_getfield(bf, &obj3, obj, "tpage")
&& blend_object_getdata(bf, &tpageptr, obj3)))
{
mesh->face[i].m_image = 0;
for (j=0; j<4; ++j)
{
uint32_t uldata;
for (k=0; k<2; ++k) {
float fdata;
if (!(blend_object_structure_getfield(bf, &aobj, obj, "uv") &&
blend_object_array_getdata(bf, &fdata, aobj, j,k))) {
abort();
}
mesh->face[i].uv[j][k] = fdata;
}
mesh->face[i].uv[j][1] = 1.0f - mesh->face[i].uv[j][1];
/* texture face colour... not sure how this conceptually
differs from the face vertex colours, but it does. */
/* in its usual inconsistant style, blender packs this
RGBA value into the bytes of an unsigned long... */
}
//printf("got tpage\n");
{
int k;
for (k=0;k<gNumCachedImages;k++)
{
if (tCachedTPage[k]==tpageptr)
{
mesh->face[i].m_image=tCachedImages[k];
break;
}
}
}
if (!mesh->face[i].m_image)
{
bImage* bimg= (bImage*)malloc (sizeof(bImage));
tCachedImages[gNumCachedImages]=bimg;
tCachedTPage[gNumCachedImages] = tpageptr;
gNumCachedImages++;
mesh->face[i].m_image = bimg;
bimg->m_packedImagePtr = 0;
bimg->m_sizePackedImage = 0;
if (tpageptr)
{
BlendObject name_block;
BlendObject tpage = blend_block_get_object(bf,tpageptr,0);
BlendObject okblock;
if ((blend_object_structure_getfield(bf, &okblock, tpage, "ok")))
{
short int okval;
if (blend_object_getdata(bf,&okval,okblock))
{
// printf("ok=%d\n",okval);
bimg->m_ok = okval;
} else
{
bimg->m_ok=0;
}
}
if ((blend_object_structure_getfield(bf, &okblock, tpage, "xrep")))
{
short int xrep;
if (blend_object_getdata(bf,&xrep,okblock))
{
// printf("xrep=%d\n",xrep);
bimg->m_xrep = xrep;
} else
{
bimg->m_xrep = 0;
}
}
if ((blend_object_structure_getfield(bf, &okblock, tpage, "yrep")))
{
short int yrep;
if (blend_object_getdata(bf,&yrep,okblock))
{
// printf("yrep=%d\n",yrep);
bimg->m_yrep = yrep;
} else
{
bimg->m_yrep = 0;
}
}
mesh->face[i].image_id = 0;
{
BlendBlockPointer packptr;
if ((blend_object_structure_getfield(bf, &obj3, tpage, "packedfile")
&& blend_object_getdata(bf, &packptr, obj3)))
{
if (packptr)
{
BlendObject packfile= blend_block_get_object(bf,packptr,0);
BlendBlockPointer dataptr;
if ((blend_object_structure_getfield(bf, &obj3, packfile, "data")
&& blend_object_getdata(bf, &dataptr, obj3)))
{
/*BlendObject data= blend_block_get_object(bf,dataptr,0);
char dest[1024];
blend_object_getstring(bf,data,dest,1023);
*/
// printf("...\n");
//if (blend_object_structure_getfield(bf, &obj2, ipo,"curve"))
{
BlendBlock* block = (BlendBlock*)obj3.block;
void** ptrptr = &block->array_entries->field_bytes[block->array_entries->field_offsets[obj3.field_index]];
BlendBlockPointer ptr = *ptrptr;
if (ptr)
{
BlendBlockPointer curveblockptr = blend_block_from_blendpointer(bf, ptr);
BlendObject curve = blend_block_get_object(bf, curveblockptr, 0);
BlendBlock* bb = (BlendBlock* )curve.block;
BlendBlock imgblock;
mesh->face[i].image_id = bb->blender_pointer;
//imgblock = blend_block_get_object(bf, bb->blender_pointer, 0);
bimg->m_packedImagePtr = bb->customData;
bimg->m_sizePackedImage = bb->customDataSize;
}
}
}
}
}
}
if ((blend_object_structure_getfield(bf, &name_block, tpage, "name")))
{
int max_chars=127;
if (blend_object_getstring(bf,name_block,bimg->m_imagePathName, max_chars))
{
// printf("texname=%s\n",bimg->m_imagePathName);
}
}
}
}
}
}
}
else
{
if (ttblock) {
/* we have tex co-ords */ /* we have tex co-ords */
for (i=0; i<mesh->face_count; ++i) { for (i=0; i<mesh->face_count; ++i) {
int j,k; int j,k;
void *pdata; void *pdata;
BlendObject aobj; BlendObject aobj;
BlendObject obj = blend_block_get_object(bf, tblock, i); BlendObject obj = blend_block_get_object(bf, ttblock, i);
if (!(blend_object_structure_getfield(bf, &aobj, obj, "tpage") && if (!(blend_object_structure_getfield(bf, &aobj, obj, "tpage") &&
blend_object_getdata(bf, &pdata, aobj))) { blend_object_getdata(bf, &pdata, aobj))) {
@@ -2268,6 +2657,15 @@ blend_acquire_mesh_from_obj(BlendFile *bf, BlendObject *meobj, bMesh *mesh)
} }
} }
} }
}
if (mtblock)
{
BlendObject obj = blend_block_get_object(bf, mtblock, 0);
//6155
}
if (dblock) { if (dblock) {
/* we have vertex deformation weights */ /* we have vertex deformation weights */

49
Extras/readblend/readblend.h
View File

@@ -38,11 +38,11 @@
/* TODO: Doxygen me. */ /* TODO: Doxygen me. */
/* don't worry yourself about this. */ #include "blendtype.h"
#ifndef BlendFile
#define BlendFile void
#endif
#ifdef __cplusplus
extern "C" {
#endif
@@ -101,6 +101,7 @@ typedef struct {
long entry_index; long field_index; long entry_index; long field_index;
} BlendObject; } BlendObject;
/* The callback type for passing to blend_foreach_block() (the callback /* The callback type for passing to blend_foreach_block() (the callback
should return zero if it doesn't want to see any more blocks.) */ should return zero if it doesn't want to see any more blocks.) */
#define BLENDBLOCKCALLBACK_RETURN int #define BLENDBLOCKCALLBACK_RETURN int
@@ -312,20 +313,41 @@ typedef struct {
bDeformWeight* deform_weights; bDeformWeight* deform_weights;
int deform_weights_count; int deform_weights_count;
} bVert; } bVert;
#define BVERT_HAS_CNORMAL(BV) ((BV)->cnormal[0] != 0.0f || \ #define BVERT_HAS_CNORMAL(BV) ((BV)->cnormal[0] != 0.0f || \
(BV)->cnormal[1] != 0.0f || \ (BV)->cnormal[1] != 0.0f || \
(BV)->cnormal[2] != 0.0f) (BV)->cnormal[2] != 0.0f)
#define BFACE_FLAG_SMOOTH 0x01 #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 { typedef struct {
int v[4]; int v[4];
float rgba[4][4]; /* vertex colours */ float rgba[4][4]; /* vertex colours */
float rgba2[4][4]; /* texture face colours (errrr...?) */ float rgba2[4][4]; /* texture face colours (errrr...?) */
float uv[4][2]; float uv[4][2];
BlendBlockPointer image_id; bImage* m_image;
int mat; int mat;
unsigned char flags; unsigned char flags;
int m_flag;
int m_mode;
BlendBlockPointer image_id;
} bFace; } bFace;
#define BFACE_HAS_TEXTURE(BF) ((BF)->image_id != NULL) #define BFACE_HAS_TEXTURE(BF) ((BF)->image_id != NULL)
#define BFACE_IS_QUAD(BF) ((BF)->v[3] != 0) #define BFACE_IS_QUAD(BF) ((BF)->v[3] != 0)
#define BFACE_IS_TRI(BF) ((BF)->v[2] != 0 && (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 float bMatrix[4][4];
typedef struct { typedef struct _bMesh{
bVert *vert; bVert *vert;
int vert_count; int vert_count;
bFace *face; bFace *face;
@@ -400,14 +422,17 @@ typedef struct {
typedef enum { typedef enum {
BOBJ_TYPE_UNKNOWN, BOBJ_TYPE_UNKNOWN,
BOBJ_TYPE_NULL, /* indicates object has no data associated with it! */ 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; } bObjType;
typedef enum { typedef enum {
BAQ_INCLUDE_CHILDREN = 0x0001 BAQ_INCLUDE_CHILDREN = 0x0001
} bAcquireFlags; } bAcquireFlags;
typedef struct { typedef struct _bObj{
bObjType type; bObjType type;
char *name; char *name;
@@ -417,11 +442,15 @@ typedef struct {
float rotphr[3]; /* pitch/head/roll rotation component of transform (use for normals) */ float rotphr[3]; /* pitch/head/roll rotation component of transform (use for normals) */
float location[3]; /* location component of transform */ float location[3]; /* location component of transform */
unsigned char transflags; /* NOT DECODED YET, RAW BYTE */ 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 { union {
void *dummy; void *dummy;
bMesh *mesh; bMesh *mesh;
} data; } data;
} bObj; } bObj;
@@ -456,4 +485,8 @@ void blend_free_texlayer(bTexLayer *tl);
void blend_acquire_texlayer_from_obj(BlendFile *bf, BlendObject *tlobj, void blend_acquire_texlayer_from_obj(BlendFile *bf, BlendObject *tlobj,
bTexLayer *tl); bTexLayer *tl);
#ifdef __cplusplus
}
#endif
#endif #endif

97
Extras/readblend/testblend.c
View File

@@ -4,38 +4,101 @@
#include "abs-file.h" #include "abs-file.h"
#include "readblend.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, void crawl(BlendFile* blend_file,
BlendObject obj) BlendObject obj)
{ {
BlendObject data_obj; BlendObject data_obj;
BlendObject data_obj2;
if (blend_object_structure_getfield(blend_file, &data_obj, obj, "totvert")) { BlendBlock* tmpBlock=0;
if (blend_object_type(blend_file, data_obj) == BLEND_OBJ_LONG32) {
long data1 = 123456; {
if (blend_object_getdata(blend_file, &data1, data_obj)) { const char* type_name = blend_file->types[obj.type].name;
fprintf(stderr, "TOTVERT=(%ld) ", data1); if (strcmp(type_name,"Object")==0)
} else { {
fprintf(stderr, "FETCH ERROR\n"); 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 (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) { if (blend_object_type(blend_file, data_obj) == BLEND_OBJ_FLOAT) {
float data1 = 123456.7; float data1 = 123456.7;
float data2 = 123456.8; if (blend_object_getdata(blend_file, &data1, data_obj))
float data3 = 123456.9; fprintf(stderr, "gravity=(%f) ", data1);
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 { } else {
fprintf(stderr, "FETCH ERROR\n"); fprintf(stderr, "FETCH ERROR\n");
} }
}
}
} }
} }
}
}
}
} }
static BLENDBLOCKCALLBACK_RETURN static BLENDBLOCKCALLBACK_RETURN
@@ -86,7 +149,7 @@ main(int argc, char *argv[])
blend_dump_typedefs(bf); blend_dump_typedefs(bf);
blend_dump_blocks(bf); // blend_dump_blocks(bf);
fflush(stdout); fflush(stdout);
@@ -104,7 +167,7 @@ main(int argc, char *argv[])
blend_free(bf); blend_free(bf);
closeit: closeit:
MY_CLOSE(file); MY_CLOSE(file);
return rtn; return rtn;
} }

20
src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
View File

@@ -207,12 +207,24 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
input.m_transformB = body1->getWorldTransform(); input.m_transformB = body1->getWorldTransform();
gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); 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 v0,v1;
btVector3 sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized(); 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); btPlaneSpace1(sepNormalWorldSpace,v0,v1);
}
}
#endif //USE_SEPDISTANCE_UTIL2
//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
//perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points //perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
@@ -280,7 +292,7 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
#ifdef USE_SEPDISTANCE_UTIL2 #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()); m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0->getWorldTransform(),body1->getWorldTransform());
} }

55
src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
View File

@@ -29,6 +29,12 @@ Written by: Marcus Hennix
#define CONETWIST_DEF_FIX_THRESH btScalar(.05f) #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() btConeTwistConstraint::btConeTwistConstraint()
:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE), :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE),
@@ -70,8 +76,6 @@ void btConeTwistConstraint::init()
} }
void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info) void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
{ {
if (m_useSolveConstraintObsolete) if (m_useSolveConstraintObsolete)
@@ -83,7 +87,7 @@ void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
{ {
info->m_numConstraintRows = 3; info->m_numConstraintRows = 3;
info->nub = 3; info->nub = 3;
calcAngleInfo2(); calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
if(m_solveSwingLimit) if(m_solveSwingLimit)
{ {
info->m_numConstraintRows++; info->m_numConstraintRows++;
@@ -102,21 +106,30 @@ 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) 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); btAssert(!m_useSolveConstraintObsolete);
//retrieve matrices
btTransform body0_trans;
body0_trans = m_rbA.getCenterOfMassTransform();
btTransform body1_trans;
body1_trans = m_rbB.getCenterOfMassTransform();
// set jacobian // set jacobian
info->m_J1linearAxis[0] = 1; info->m_J1linearAxis[0] = 1;
info->m_J1linearAxis[info->rowskip+1] = 1; info->m_J1linearAxis[info->rowskip+1] = 1;
info->m_J1linearAxis[2*info->rowskip+2] = 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* angular0 = (btVector3*)(info->m_J1angularAxis);
btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip); btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
@@ -124,7 +137,7 @@ void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info)
btVector3 a1neg = -a1; btVector3 a1neg = -a1;
a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); 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* angular0 = (btVector3*)(info->m_J2angularAxis);
btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip); btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
@@ -136,7 +149,7 @@ void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info)
int j; int j;
for (j=0; j<3; 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_lowerLimit[j*info->rowskip] = -SIMD_INFINITY;
info->m_upperLimit[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_swingCorrection = btScalar(0.);
m_twistLimitSign = btScalar(0.); m_twistLimitSign = btScalar(0.);
@@ -606,8 +619,8 @@ void btConeTwistConstraint::calcAngleInfo2()
// TODO : split rotation to pure swing and pure twist // TODO : split rotation to pure swing and pure twist
// compute desired transforms in world // compute desired transforms in world
btTransform trPose(m_qTarget); btTransform trPose(m_qTarget);
btTransform trA = getRigidBodyA().getCenterOfMassTransform() * m_rbAFrame; btTransform trA = transA * m_rbAFrame;
btTransform trB = getRigidBodyB().getCenterOfMassTransform() * m_rbBFrame; btTransform trB = transB * m_rbBFrame;
btTransform trDeltaAB = trB * trPose * trA.inverse(); btTransform trDeltaAB = trB * trPose * trA.inverse();
btQuaternion qDeltaAB = trDeltaAB.getRotation(); btQuaternion qDeltaAB = trDeltaAB.getRotation();
btVector3 swingAxis = btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z()); 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) // compute rotation of A wrt B (in constraint space)
btQuaternion qA = getRigidBodyA().getCenterOfMassTransform().getRotation() * m_rbAFrame.getRotation(); btQuaternion qA = transA.getRotation() * m_rbAFrame.getRotation();
btQuaternion qB = getRigidBodyB().getCenterOfMassTransform().getRotation() * m_rbBFrame.getRotation(); btQuaternion qB = transB.getRotation() * m_rbBFrame.getRotation();
btQuaternion qAB = qB.inverse() * qA; btQuaternion qAB = qB.inverse() * qA;
// split rotation into cone and twist // split rotation into cone and twist
// (all this is done from B's perspective. Maybe I should be averaging axes...) // (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; // 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?) // 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 // anyway, we have either hinge or fixed joint
btVector3 ivA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0); btVector3 ivA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
btVector3 jvA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1); btVector3 jvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1);
btVector3 kvA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); btVector3 kvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(2);
btVector3 ivB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(0); btVector3 ivB = transB.getBasis() * m_rbBFrame.getBasis().getColumn(0);
btVector3 target; btVector3 target;
btScalar x = ivB.dot(ivA); btScalar x = ivB.dot(ivA);
btScalar y = ivB.dot(jvA); btScalar y = ivB.dot(jvA);

5
src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h
View File

@@ -111,8 +111,11 @@ public:
virtual void getInfo1 (btConstraintInfo1* info); virtual void getInfo1 (btConstraintInfo1* info);
void getInfo1NonVirtual(btConstraintInfo1* info);
virtual void getInfo2 (btConstraintInfo2* 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); virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep);
@@ -198,7 +201,7 @@ public:
} }
void calcAngleInfo(); void calcAngleInfo();
void calcAngleInfo2(); void calcAngleInfo2(const btTransform& transA, const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB);
inline btScalar getSwingSpan1() inline btScalar getSwingSpan1()
{ {

394
src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
View File

@@ -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 "btContactConstraint.h"
#include "BulletDynamics/Dynamics/btRigidBody.h" #include "BulletDynamics/Dynamics/btRigidBody.h"
#include "LinearMath/btVector3.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.);
}

115
src/BulletDynamics/ConstraintSolver/btContactConstraint.h
View File

@@ -16,107 +16,56 @@ subject to the following restrictions:
#ifndef CONTACT_CONSTRAINT_H #ifndef CONTACT_CONSTRAINT_H
#define 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/btVector3.h"
#include "LinearMath/btScalar.h" #include "btJacobianEntry.h"
struct btContactSolverInfo; #include "btTypedConstraint.h"
class btManifoldPoint; #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
enum { ///btContactConstraint can be automatically created to solve contact constraints using the unified btTypedConstraint interface
DEFAULT_CONTACT_SOLVER_TYPE=0, ATTRIBUTE_ALIGNED16(class) btContactConstraint : public btTypedConstraint
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
{ {
inline btConstraintPersistentData() protected:
:m_appliedImpulse(btScalar(0.)),
m_prevAppliedImpulse(btScalar(0.)), btPersistentManifold m_contactManifold;
m_accumulatedTangentImpulse0(btScalar(0.)),
m_accumulatedTangentImpulse1(btScalar(0.)), public:
m_jacDiagABInv(btScalar(0.)),
m_persistentLifeTime(0), btContactConstraint();
m_restitution(btScalar(0.)),
m_friction(btScalar(0.)), btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB);
m_penetration(btScalar(0.)),
m_contactSolverFunc(0), void setContactManifold(btPersistentManifold* contactManifold);
m_frictionSolverFunc(0)
btPersistentManifold* getContactManifold()
{ {
return &m_contactManifold;
} }
const btPersistentManifold* getContactManifold() const
{
return &m_contactManifold;
}
/// total applied impulse during most recent frame virtual ~btContactConstraint();
btScalar m_appliedImpulse;
btScalar m_prevAppliedImpulse;
btScalar m_accumulatedTangentImpulse0;
btScalar m_accumulatedTangentImpulse1;
btScalar m_jacDiagABInv; virtual void getInfo1 (btConstraintInfo1* info);
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; virtual void getInfo2 (btConstraintInfo2* info);
btVector3 m_frictionAngularComponent0B;
btVector3 m_frictionAngularComponent1A;
btVector3 m_frictionAngularComponent1B;
//some data doesn't need to be persistent over frames: todo: clean/reuse this ///obsolete methods
btVector3 m_angularComponentA; virtual void buildJacobian();
btVector3 m_angularComponentB;
ContactSolverFunc m_contactSolverFunc; ///obsolete methods
ContactSolverFunc m_frictionSolverFunc; 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, void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
btRigidBody& body2, const btVector3& pos2, btRigidBody& body2, const btVector3& pos2,
btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep); 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 #endif //CONTACT_CONSTRAINT_H

396
src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
View File

@@ -26,7 +26,7 @@ subject to the following restrictions:
#define HINGE_USE_OBSOLETE_SOLVER false #define HINGE_USE_OBSOLETE_SOLVER false
#ifndef __SPU__
btHingeConstraint::btHingeConstraint() btHingeConstraint::btHingeConstraint()
: btTypedConstraint (HINGE_CONSTRAINT_TYPE), : btTypedConstraint (HINGE_CONSTRAINT_TYPE),
@@ -249,7 +249,7 @@ void btHingeConstraint::buildJacobian()
m_accLimitImpulse = btScalar(0.); m_accLimitImpulse = btScalar(0.);
// test angular limit // test angular limit
testLimit(); testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
//Compute K = J*W*J' for hinge axis //Compute K = J*W*J' for hinge axis
btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); 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) void btHingeConstraint::getInfo1(btConstraintInfo1* info)
@@ -272,52 +432,77 @@ void btHingeConstraint::getInfo1(btConstraintInfo1* info)
{ {
info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular
info->nub = 1; info->nub = 1;
//always add the row, to avoid computation (data is not available yet)
//prepare constraint //prepare constraint
testLimit(); testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
if(getSolveLimit() || getEnableAngularMotor()) if(getSolveLimit() || getEnableAngularMotor())
{ {
info->m_numConstraintRows++; // limit 3rd anguar as well info->m_numConstraintRows++; // limit 3rd anguar as well
info->nub--; 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) 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); btAssert(!m_useSolveConstraintObsolete);
int i, s = info->rowskip; int i, skip = info->rowskip;
// transforms in world space // transforms in world space
btTransform trA = m_rbA.getCenterOfMassTransform()*m_rbAFrame; btTransform trA = transA*m_rbAFrame;
btTransform trB = m_rbB.getCenterOfMassTransform()*m_rbBFrame; btTransform trB = transB*m_rbBFrame;
// pivot point // pivot point
btVector3 pivotAInW = trA.getOrigin(); btVector3 pivotAInW = trA.getOrigin();
btVector3 pivotBInW = trB.getOrigin(); btVector3 pivotBInW = trB.getOrigin();
// linear (all fixed) // linear (all fixed)
info->m_J1linearAxis[0] = 1; info->m_J1linearAxis[0] = 1;
info->m_J1linearAxis[s + 1] = 1; info->m_J1linearAxis[skip + 1] = 1;
info->m_J1linearAxis[2 * s + 2] = 1; info->m_J1linearAxis[2 * skip + 2] = 1;
btVector3 a1 = pivotAInW - m_rbA.getCenterOfMassTransform().getOrigin();
btVector3 a1 = pivotAInW - transA.getOrigin();
{ {
btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + s); btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + skip);
btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * s); btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * skip);
btVector3 a1neg = -a1; btVector3 a1neg = -a1;
a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
} }
btVector3 a2 = pivotBInW - m_rbB.getCenterOfMassTransform().getOrigin(); btVector3 a2 = pivotBInW - transB.getOrigin();
{ {
btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + s); btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + skip);
btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * s); btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * skip);
a2.getSkewSymmetricMatrix(angular0,angular1,angular2); a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
} }
// linear RHS // linear RHS
btScalar k = info->fps * info->erp; btScalar k = info->fps * info->erp;
for(i = 0; i < 3; i++) 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 // make rotations around X and Y equal
// the hinge axis should be the only unconstrained // the hinge axis should be the only unconstrained
@@ -436,8 +621,8 @@ void btHingeConstraint::getInfo2 (btConstraintInfo2* info)
btScalar bounce = m_relaxationFactor; btScalar bounce = m_relaxationFactor;
if(bounce > btScalar(0.0)) if(bounce > btScalar(0.0))
{ {
btScalar vel = m_rbA.getAngularVelocity().dot(ax1); btScalar vel = angVelA.dot(ax1);
vel -= m_rbB.getAngularVelocity().dot(ax1); vel -= angVelB.dot(ax1);
// only apply bounce if the velocity is incoming, and if the // only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have. // resulting c[] exceeds what we already have.
if(limit == 1) 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() btScalar btHingeConstraint::getHingeAngle()
{ {
const btVector3 refAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0); return getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
const btVector3 refAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1); }
const btVector3 swingAxis = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(1);
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)); btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
return m_referenceSign * angle; return m_referenceSign * angle;
} }
@@ -676,10 +708,10 @@ void btHingeConstraint::testLimit()
#else #else
void btHingeConstraint::testLimit() void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& transB)
{ {
// Compute limit information // Compute limit information
m_hingeAngle = getHingeAngle(); m_hingeAngle = getHingeAngle(transA,transB);
m_correction = btScalar(0.); m_correction = btScalar(0.);
m_limitSign = btScalar(0.); m_limitSign = btScalar(0.);
m_solveLimit = false; m_solveLimit = false;
@@ -741,7 +773,7 @@ void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt)
} }
// compute angular velocity // compute angular velocity
btScalar curAngle = getHingeAngle(); btScalar curAngle = getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
btScalar dAngle = targetAngle - curAngle; btScalar dAngle = targetAngle - curAngle;
m_motorTargetVelocity = dAngle / dt; m_motorTargetVelocity = dAngle / dt;
} }

10
src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
View File

@@ -26,7 +26,7 @@ class btRigidBody;
/// hinge constraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space /// 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 /// axis defines the orientation of the hinge axis
class btHingeConstraint : public btTypedConstraint ATTRIBUTE_ALIGNED16(class) btHingeConstraint : public btTypedConstraint
{ {
#ifdef IN_PARALLELL_SOLVER #ifdef IN_PARALLELL_SOLVER
public: public:
@@ -81,8 +81,12 @@ public:
virtual void getInfo1 (btConstraintInfo1* info); virtual void getInfo1 (btConstraintInfo1* info);
void getInfo1NonVirtual(btConstraintInfo1* info);
virtual void getInfo2 (btConstraintInfo2* 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); virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep);
void updateRHS(btScalar timeStep); void updateRHS(btScalar timeStep);
@@ -174,7 +178,9 @@ public:
btScalar getHingeAngle(); 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; }; const btTransform& getAFrame() const { return m_rbAFrame; };

2
src/BulletDynamics/ConstraintSolver/btJacobianEntry.h
View File

@@ -28,7 +28,7 @@ subject to the following restrictions:
/// Jacobian entry is an abstraction that allows to describe constraints /// Jacobian entry is an abstraction that allows to describe constraints
/// it can be used in combination with a constraint solver /// it can be used in combination with a constraint solver
/// Can be used to relate the effect of an impulse to the constraint error /// Can be used to relate the effect of an impulse to the constraint error
class btJacobianEntry ATTRIBUTE_ALIGNED16(class) btJacobianEntry
{ {
public: public:
btJacobianEntry() {}; btJacobianEntry() {};

22
src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp
View File

@@ -43,6 +43,7 @@ m_useSolveConstraintObsolete(false)
void btPoint2PointConstraint::buildJacobian() void btPoint2PointConstraint::buildJacobian()
{ {
///we need it for both methods ///we need it for both methods
{ {
m_appliedImpulse = btScalar(0.); m_appliedImpulse = btScalar(0.);
@@ -66,10 +67,15 @@ void btPoint2PointConstraint::buildJacobian()
} }
} }
} }
void btPoint2PointConstraint::getInfo1 (btConstraintInfo1* info) void btPoint2PointConstraint::getInfo1 (btConstraintInfo1* info)
{
getInfo1NonVirtual(info);
}
void btPoint2PointConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
{ {
if (m_useSolveConstraintObsolete) if (m_useSolveConstraintObsolete)
{ {
@@ -82,15 +88,19 @@ void btPoint2PointConstraint::getInfo1 (btConstraintInfo1* info)
} }
} }
void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* 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); btAssert(!m_useSolveConstraintObsolete);
//retrieve matrices //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. // anchor points in global coordinates with respect to body PORs.
@@ -150,6 +160,7 @@ void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* info)
void btPoint2PointConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) void btPoint2PointConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
{ {
if (m_useSolveConstraintObsolete) if (m_useSolveConstraintObsolete)
{ {
btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA; btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
@@ -220,6 +231,7 @@ void btPoint2PointConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolv
normal[i] = 0; normal[i] = 0;
} }
} }
} }
void btPoint2PointConstraint::updateRHS(btScalar timeStep) void btPoint2PointConstraint::updateRHS(btScalar timeStep)

5
src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h
View File

@@ -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 /// 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 #ifdef IN_PARALLELL_SOLVER
public: public:
@@ -65,8 +65,11 @@ public:
virtual void getInfo1 (btConstraintInfo1* info); virtual void getInfo1 (btConstraintInfo1* info);
void getInfo1NonVirtual (btConstraintInfo1* info);
virtual void getInfo2 (btConstraintInfo2* 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); virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep);

2
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
View File

@@ -22,7 +22,7 @@ class btIDebugDraw;
#include "btSolverBody.h" #include "btSolverBody.h"
#include "btSolverConstraint.h" #include "btSolverConstraint.h"
#include "btTypedConstraint.h" #include "btTypedConstraint.h"
#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method. ///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method.
class btSequentialImpulseConstraintSolver : public btConstraintSolver class btSequentialImpulseConstraintSolver : public btConstraintSolver

54
src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp
View File

@@ -126,6 +126,7 @@ void btSliderConstraint::buildJacobian()
void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB) void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB)
{ {
#ifndef __SPU__
//calculate transforms //calculate transforms
m_calculatedTransformA = rbA.getCenterOfMassTransform() * frameInA; m_calculatedTransformA = rbA.getCenterOfMassTransform() * frameInA;
m_calculatedTransformB = rbB.getCenterOfMassTransform() * frameInB; m_calculatedTransformB = rbB.getCenterOfMassTransform() * frameInB;
@@ -175,10 +176,10 @@ void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, co
// clear accumulator for motors // clear accumulator for motors
m_accumulatedLinMotorImpulse = btScalar(0.0); m_accumulatedLinMotorImpulse = btScalar(0.0);
m_accumulatedAngMotorImpulse = btScalar(0.0); m_accumulatedAngMotorImpulse = btScalar(0.0);
#endif //__SPU__
} }
void btSliderConstraint::getInfo1(btConstraintInfo1* info) void btSliderConstraint::getInfo1(btConstraintInfo1* info)
{ {
if (m_useSolveConstraintObsolete) if (m_useSolveConstraintObsolete)
@@ -191,7 +192,7 @@ void btSliderConstraint::getInfo1(btConstraintInfo1* info)
info->m_numConstraintRows = 4; // Fixed 2 linear + 2 angular info->m_numConstraintRows = 4; // Fixed 2 linear + 2 angular
info->nub = 2; info->nub = 2;
//prepare constraint //prepare constraint
calculateTransforms(); calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
testLinLimits(); testLinLimits();
if(getSolveLinLimit() || getPoweredLinMotor()) 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) void btSliderConstraint::getInfo2(btConstraintInfo2* info)
{ {
btAssert(!m_useSolveConstraintObsolete);
int i, s = info->rowskip;
const btTransform& trA = getCalculatedTransformA(); const btTransform& trA = getCalculatedTransformA();
const btTransform& trB = getCalculatedTransformB(); 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); btScalar signFact = m_useLinearReferenceFrameA ? btScalar(1.0f) : btScalar(-1.0f);
// make rotations around Y and Z equal // make rotations around Y and Z equal
// the slider axis should be the only unconstrained // 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 // result in three equations, so we project along the planespace vectors
// so that sliding along the slider axis is disregarded. for symmetry we // 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). // also consider rotation around center of mass of two bodies (factA and factB).
btTransform bodyA_trans = m_rbA.getCenterOfMassTransform(); btTransform bodyA_trans = transA;
btTransform bodyB_trans = m_rbB.getCenterOfMassTransform(); btTransform bodyB_trans = transB;
int s2 = 2 * s, s3 = 3 * s; int s2 = 2 * s, s3 = 3 * s;
btVector3 c; btVector3 c;
btScalar miA = m_rbA.getInvMass(); btScalar miA = rbAinvMass;
btScalar miB = m_rbB.getInvMass(); btScalar miB = rbBinvMass;
btScalar miS = miA + miB; btScalar miS = miA + miB;
btScalar factA, factB; btScalar factA, factB;
if(miS > btScalar(0.f)) if(miS > btScalar(0.f))
@@ -389,8 +410,8 @@ void btSliderConstraint::getInfo2(btConstraintInfo2* info)
btScalar bounce = btFabs(btScalar(1.0) - getDampingLimLin()); btScalar bounce = btFabs(btScalar(1.0) - getDampingLimLin());
if(bounce > btScalar(0.0)) if(bounce > btScalar(0.0))
{ {
btScalar vel = m_rbA.getLinearVelocity().dot(ax1); btScalar vel = linVelA.dot(ax1);
vel -= m_rbB.getLinearVelocity().dot(ax1); vel -= linVelB.dot(ax1);
vel *= signFact; vel *= signFact;
// only apply bounce if the velocity is incoming, and if the // only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have. // 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) void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB)
{ {
#ifndef __SPU__
int i; int i;
// linear // linear
btVector3 velA; btVector3 velA;
@@ -719,22 +741,24 @@ void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btSolverBody& body
bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-angImpulse); 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)) if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete))
{ {
m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA; m_calculatedTransformA = transA * m_frameInA;
m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB; m_calculatedTransformB = transB * m_frameInB;
} }
else else
{ {
m_calculatedTransformA = m_rbB.getCenterOfMassTransform() * m_frameInB; m_calculatedTransformA = transB * m_frameInB;
m_calculatedTransformB = m_rbA.getCenterOfMassTransform() * m_frameInA; m_calculatedTransformB = transA * m_frameInA;
} }
m_realPivotAInW = m_calculatedTransformA.getOrigin(); m_realPivotAInW = m_calculatedTransformA.getOrigin();
m_realPivotBInW = m_calculatedTransformB.getOrigin(); m_realPivotBInW = m_calculatedTransformB.getOrigin();

14
src/BulletDynamics/ConstraintSolver/btSliderConstraint.h
View File

@@ -132,8 +132,12 @@ public:
virtual void buildJacobian(); virtual void buildJacobian();
virtual void getInfo1 (btConstraintInfo1* info); virtual void getInfo1 (btConstraintInfo1* info);
void getInfo1NonVirtual(btConstraintInfo1* info);
virtual void getInfo2 (btConstraintInfo2* 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); 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 buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB);
void solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB); void solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB);
// shared code used by ODE solver // shared code used by ODE solver
void calculateTransforms(void); void calculateTransforms(const btTransform& transA,const btTransform& transB);
void testLinLimits(void); void testLinLimits();
void testLinLimits2(btConstraintInfo2* info); void testLinLimits2(btConstraintInfo2* info);
void testAngLimits(void); void testAngLimits();
// access for PE Solver // access for PE Solver
btVector3 getAncorInA(void); btVector3 getAncorInA();
btVector3 getAncorInB(void); btVector3 getAncorInB();
}; };

3
src/BulletDynamics/ConstraintSolver/btTypedConstraint.h
View File

@@ -30,7 +30,8 @@ enum btTypedConstraintType
HINGE_CONSTRAINT_TYPE, HINGE_CONSTRAINT_TYPE,
CONETWIST_CONSTRAINT_TYPE, CONETWIST_CONSTRAINT_TYPE,
D6_CONSTRAINT_TYPE, D6_CONSTRAINT_TYPE,
SLIDER_CONSTRAINT_TYPE SLIDER_CONSTRAINT_TYPE,
CONTACT_CONSTRAINT_TYPE
}; };
///TypedConstraint is the baseclass for Bullet constraints and vehicles ///TypedConstraint is the baseclass for Bullet constraints and vehicles

5
src/BulletMultiThreaded/SequentialThreadSupport.h
View File

@@ -81,6 +81,11 @@ public:
virtual void setNumTasks(int numTasks); virtual void setNumTasks(int numTasks);
virtual int getNumTasks() const
{
return 1;
}
}; };
#endif //SEQUENTIAL_THREAD_SUPPORT_H #endif //SEQUENTIAL_THREAD_SUPPORT_H

2
src/BulletMultiThreaded/Win32ThreadSupport.cpp
View File

@@ -183,6 +183,8 @@ void Win32ThreadSupport::startThreads(const Win32ThreadConstructionInfo& threadC
m_activeSpuStatus.resize(threadConstructionInfo.m_numThreads); m_activeSpuStatus.resize(threadConstructionInfo.m_numThreads);
m_completeHandles.resize(threadConstructionInfo.m_numThreads); m_completeHandles.resize(threadConstructionInfo.m_numThreads);
m_maxNumTasks = threadConstructionInfo.m_numThreads;
for (int i=0;i<threadConstructionInfo.m_numThreads;i++) for (int i=0;i<threadConstructionInfo.m_numThreads;i++)
{ {
printf("starting thread %d\n",i); printf("starting thread %d\n",i);