admin/opensim-libs
1
0
Fork 0
mirror of git://opensimulator.org/git/opensim-libs synced 2026-08-14 00:58:00 +00:00
Code Issues Packages Projects Releases Wiki Activity

more on ubode

Browse Source
This commit is contained in:
UbitUmarov
2022-08-05 02:16:28 +01:00
parent 23034c9b61
commit dfb564e6c1
16 changed files with 155 additions and 217 deletions
Download Patch File Download Diff File Expand all files Collapse all files
trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_MeshInterface.h
+2 -2
View File
@@ -63,8 +63,8 @@ public:
return mVerts;
}
inline_ bool SetStrides(udword tri_stride = sizeof(IndexedTriangle), udword vertex_stride = sizeof(Point)) { return true; }
inline_ void SetSingle(bool value) {}
//inline_ bool SetStrides(udword tri_stride = sizeof(IndexedTriangle), udword vertex_stride = sizeof(Point)) { return true; }
//inline_ void SetSingle(bool value) {}
inline_ udword GetTriStride() const { return sizeof(IndexedTriangle); }
inline_ udword GetVertexStride() const { return sizeof(Point); }
trunk/unmanaged/ubODE-OpenSim/include/ode/collision.h
+1 -1
View File
@@ -452,7 +452,7 @@ enum
* @returns Boolean execution status
* @ingroup collide
*/
ODE_API int dGeomLowLevelControl (dGeomID geom, int controlClass, int controlCode, void *dataValue, int *dataSize);
//ODE_API int dGeomLowLevelControl (dGeomID geom, int controlClass, int controlCode, void *dataValue, int *dataSize);
/**
trunk/unmanaged/ubODE-OpenSim/include/ode/odeconfig.h
-15
View File
@@ -141,23 +141,8 @@
#endif
#endif
/* Visual C does not define these functions */
#if defined(_MSC_VER)
#define _ode_copysignf(x, y) ((float)_copysign(x, y))
#define _ode_copysign(x, y) _copysign(x, y)
#define _ode_nextafterf(x, y) _nextafterf(x, y)
#define _ode_nextafter(x, y) _nextafter(x, y)
#if !defined(_WIN64) && defined(dSINGLE)
#define _ODE__NEXTAFTERF_REQUIRED
ODE_EXTERN_C float _nextafterf(float x, float y);
#endif
#else
#define _ode_copysignf(x, y) copysignf(x, y)
#define _ode_copysign(x, y) copysign(x, y)
#define _ode_nextafterf(x, y) nextafterf(x, y)
#define _ode_nextafter(x, y) nextafter(x, y)
#endif
#endif
trunk/unmanaged/ubODE-OpenSim/ode/src/box.cpp
+2 -2
View File
@@ -817,8 +817,8 @@ int dBoxBox (const dVector3 p1, const dMatrix3 R1,
for (j=0; j < n; j++)
{
dReal k1 = m22 * (ret[j * 2] - c1) - m12 * (ret[j * 2 + 1] - c2);
dReal k2 = -m21 * (ret[j * 2] - c1) + m11 * (ret[j * 2 + 1] - c2);
k1 = m22 * (ret[j * 2] - c1) - m12 * (ret[j * 2 + 1] - c2);
k2 = -m21 * (ret[j * 2] - c1) + m11 * (ret[j * 2 + 1] - c2);
for (i = 0; i < 3; i++)
point[cnum * 3 + i] = center[i] + k1 * Rb[i * 4 + a1] + k2 * Rb[i * 4 + a2];
dep[cnum] = Sa[codeN] - dCalcVectorDot3(normal2, point + cnum * 3);
trunk/unmanaged/ubODE-OpenSim/ode/src/collision_kernel.cpp
+10 -8
View File
@@ -471,13 +471,15 @@ void dxGeom::computePosr()
dMultiply0_333 (final_posr->R, body->posr.R, offset_posr->R);
}
bool dxGeom::controlGeometry(int /*controlClass*/, int /*controlCode*/, void * /*dataValue*/, int *dataSize)
{
dAASSERT(false && "Control class/code is not supported for current geom");
*dataSize = 0;
return false;
}
//bool dxGeom::controlGeometry(int /*controlClass*/, int /*controlCode*/, void * /*dataValue*/, int *dataSize)
//{
// dAASSERT(false && "Control class/code is not supported for current geom");
// *dataSize = 0;
// return false;
//}
//****************************************************************************
// misc
@@ -864,7 +866,7 @@ void dGeomVectorFromWorld (dGeomID g, dReal px, dReal py, dReal pz, dVector3 res
}
/*
int dGeomLowLevelControl (dxGeom *g, int controlClass, int controlCode, void *dataValue, int *dataSize)
{
dAASSERT (g);
@@ -877,7 +879,7 @@ int dGeomLowLevelControl (dxGeom *g, int controlClass, int controlCode, void *da
bool result = g->controlGeometry(controlClass, controlCode, dataValue, dataSize);
return result;
}
*/
//****************************************************************************
// C interface that lets the user make new classes. this interface is a lot
// more cumbersome than C++ subclassing, which is what is used internally
trunk/unmanaged/ubODE-OpenSim/ode/src/collision_kernel.h
+8 -4
View File
@@ -70,9 +70,9 @@ internal data structures and functions for collision detection.
// GEOM_DIRTY|GEOM_AABB_BAD|GEOM_POSR_BAD
enum {
GEOM_DIRTY = 1, // geom is 'dirty', i.e. position unknown
GEOM_DIRTY = 1, // geom is 'dirty', i.e. position unknown
GEOM_POSR_BAD = 2, // geom's final posr is not valid
GEOM_AABB_BAD = 4, // geom's AABB is not valid
GEOM_AABB_BAD = 4, // geom's AABB is not valid
GEOM_PLACEABLE = 8, // geom is placeable
GEOM_ENABLED = 16, // geom is enabled
GEOM_ZERO_SIZED = 32, // geom is zero sized
@@ -147,8 +147,12 @@ struct dxGeom : public dBase {
return final_posr;
};
bool checkControlValueSizeValidity(void *dataValue, int *dataSize, int iRequiresSize) { return (*dataSize == iRequiresSize && dataValue != 0) ? true : !(*dataSize = iRequiresSize); } // Here it is the intent to return true for 0 required size in any case
virtual bool controlGeometry(int controlClass, int controlCode, void *dataValue, int *dataSize);
// bool checkControlValueSizeValidity(void *dataValue, int *dataSize, int iRequiresSize)
// {
// return (*dataSize == iRequiresSize && dataValue != 0) ? true : !(*dataSize = iRequiresSize);
// } // Here it is the intent to return true for 0 required size in any case
// virtual bool controlGeometry(int controlClass, int controlCode, void *dataValue, int *dataSize);
virtual void computeAABB()=0;
// compute the AABB for this object and put it in aabb. this function
trunk/unmanaged/ubODE-OpenSim/ode/src/collision_quadtreespace.cpp
+99 -145
View File
@@ -44,7 +44,8 @@ const int SPLITS = SPLITAXIS * SPLITAXIS;
#define GEOM_ENABLED(g) (((g)->gflags & GEOM_ENABLE_TEST_MASK) == GEOM_ENABLE_TEST_VALUE)
class Block{
class Block
{
public:
dReal mMinX, mMaxX;
dReal mMinZ, mMaxZ;
@@ -73,65 +74,8 @@ public:
};
#ifdef DRAWBLOCKS
#include "..\..\Include\drawstuff\\drawstuff.h"
static void DrawBlock(Block* Block){
dVector3 v[8];
v[0][AXIS0] = Block->mMinX;
v[0][UP] = REAL(-1.0);
v[0][AXIS1] = Block->mMinZ;
v[1][AXIS0] = Block->mMinX;
v[1][UP] = REAL(-1.0);
v[1][AXIS1] = Block->mMaxZ;
v[2][AXIS0] = Block->mMaxX;
v[2][UP] = REAL(-1.0);
v[2][AXIS1] = Block->mMinZ;
v[3][AXIS0] = Block->mMaxX;
v[3][UP] = REAL(-1.0);
v[3][AXIS1] = Block->mMaxZ;
v[4][AXIS0] = Block->mMinX;
v[4][UP] = REAL(1.0);
v[4][AXIS1] = Block->mMinZ;
v[5][AXIS0] = Block->mMinX;
v[5][UP] = REAL(1.0);
v[5][AXIS1] = Block->mMaxZ;
v[6][AXIS0] = Block->mMaxX;
v[6][UP] = REAL(1.0);
v[6][AXIS1] = Block->mMinZ;
v[7][AXIS0] = Block->mMaxX;
v[7][UP] = REAL(1.0);
v[7][AXIS1] = Block->mMaxZ;
// Bottom
dsDrawLine(v[0], v[1]);
dsDrawLine(v[1], v[3]);
dsDrawLine(v[3], v[2]);
dsDrawLine(v[2], v[0]);
// Top
dsDrawLine(v[4], v[5]);
dsDrawLine(v[5], v[7]);
dsDrawLine(v[7], v[6]);
dsDrawLine(v[6], v[4]);
// Sides
dsDrawLine(v[0], v[4]);
dsDrawLine(v[1], v[5]);
dsDrawLine(v[2], v[6]);
dsDrawLine(v[3], v[7]);
}
#endif //DRAWBLOCKS
void Block::Create(const dReal MinX, const dReal MaxX, const dReal MinZ, const dReal MaxZ, Block* Parent, int Depth, Block*& Blocks){
void Block::Create(const dReal MinX, const dReal MaxX, const dReal MinZ, const dReal MaxZ, Block* Parent, int Depth, Block*& Blocks)
{
dIASSERT(MinX <= MaxX);
dIASSERT(MinZ <= MaxZ);
@@ -146,7 +90,8 @@ void Block::Create(const dReal MinX, const dReal MaxX, const dReal MinZ, const d
this->mParent = Parent;
if (Depth > 0){
if (Depth > 0)
{
mChildren = Blocks;
Blocks += SPLITS;
@@ -157,12 +102,14 @@ void Block::Create(const dReal MinX, const dReal MaxX, const dReal MinZ, const d
int Index = 0;
dReal ChildRightX = MinX;
for (int i = 0; i < SPLITAXIS; i++){
for (int i = 0; i < SPLITAXIS; i++)
{
const dReal ChildLeftX = ChildRightX;
ChildRightX = (i != SPLITAXIS - 1) ? ChildLeftX + ChildExtentX : MaxX;
dReal ChildRightZ = MinZ;
for (int j = 0; j < SPLITAXIS; j++){
for (int j = 0; j < SPLITAXIS; j++)
{
const dReal ChildLeftZ = ChildRightZ;
ChildRightZ = (j != SPLITAXIS - 1) ? ChildLeftZ + ChildExtentZ : MaxZ;
@@ -175,23 +122,25 @@ void Block::Create(const dReal MinX, const dReal MaxX, const dReal MinZ, const d
}
void Block::Collide(void* UserData, dNearCallback* Callback){
#ifdef DRAWBLOCKS
DrawBlock(this);
#endif
// Collide the local list
dxGeom* g = mFirst;
while (g){
if (GEOM_ENABLED(g)){
while (g)
{
if (GEOM_ENABLED(g))
{
Collide(g, g->next_ex, UserData, Callback);
}
g = g->next_ex;
}
// Recurse for children
if (mChildren){
for (int i = 0; i < SPLITS; i++){
if (mChildren)
{
for (int i = 0; i < SPLITS; i++)
{
Block &CurrentChild = mChildren[i];
if (CurrentChild.mGeomCount <= 1){ // Early out
if (CurrentChild.mGeomCount <= 1)
{ // Early out
continue;
}
CurrentChild.Collide(UserData, Callback);
@@ -200,37 +149,39 @@ void Block::Collide(void* UserData, dNearCallback* Callback){
}
// Note: g2 is assumed to be in this Block
void Block::Collide(dxGeom* g1, dxGeom* g2, void* UserData, dNearCallback* Callback){
#ifdef DRAWBLOCKS
DrawBlock(this);
#endif
void Block::Collide(dxGeom* g1, dxGeom* g2, void* UserData, dNearCallback* Callback)
{
// Collide against local list
while (g2){
if (GEOM_ENABLED(g2)){
while (g2)
{
if (GEOM_ENABLED(g2))
{
collideAABBs (g1, g2, UserData, Callback);
}
g2 = g2->next_ex;
}
// Collide against children
if (mChildren){
for (int i = 0; i < SPLITS; i++){
if (mChildren)
{
for (int i = 0; i < SPLITS; i++)
{
Block &CurrentChild = mChildren[i];
// Early out for empty blocks
if (CurrentChild.mGeomCount == 0){
if (CurrentChild.mGeomCount == 0)
{
continue;
}
// Does the geom's AABB collide with the block?
// Don't do AABB tests for single geom blocks.
if (CurrentChild.mGeomCount == 1){
//
}
else if (true){
if (CurrentChild.mGeomCount > 1)
{
if (g1->aabb[AXIS0 * 2 + 0] >= CurrentChild.mMaxX ||
g1->aabb[AXIS0 * 2 + 1] < CurrentChild.mMinX ||
g1->aabb[AXIS1 * 2 + 0] >= CurrentChild.mMaxZ ||
g1->aabb[AXIS1 * 2 + 1] < CurrentChild.mMinZ) continue;
g1->aabb[AXIS1 * 2 + 1] < CurrentChild.mMinZ)
continue;
}
CurrentChild.Collide(g1, CurrentChild.mFirst, UserData, Callback);
}
@@ -248,7 +199,8 @@ void Block::CollideLocal(dxGeom* g2, void* UserData, dNearCallback* Callback){
}
}
void Block::AddObject(dGeomID Object){
void Block::AddObject(dGeomID Object)
{
// Add the geom
Object->next_ex = mFirst;
mFirst = Object;
@@ -256,23 +208,27 @@ void Block::AddObject(dGeomID Object){
// Now traverse upwards to tell that we have a geom
Block* Block = this;
do{
do
{
Block->mGeomCount++;
Block = Block->mParent;
}
while (Block);
}
void Block::DelObject(dGeomID Object){
void Block::DelObject(dGeomID Object)
{
// Del the geom
dxGeom* g = mFirst;
dxGeom* Last = 0;
while (g){
if (g == Object){
if (Last){
while (g)
{
if (g == Object)
{
if (Last)
Last->next_ex = g->next_ex;
}
else mFirst = g->next_ex;
else
mFirst = g->next_ex;
break;
}
@@ -284,17 +240,20 @@ void Block::DelObject(dGeomID Object){
// Now traverse upwards to tell that we have lost a geom
Block* Block = this;
do{
do
{
Block->mGeomCount--;
Block = Block->mParent;
}
while (Block);
}
void Block::Traverse(dGeomID Object){
void Block::Traverse(dGeomID Object)
{
Block* NewBlock = GetBlock(Object->aabb);
if (NewBlock != this){
if (NewBlock != this)
{
// Remove the geom from the old block and add it to the new block.
// This could be more optimal, but the loss should be very small.
DelObject(Object);
@@ -302,25 +261,36 @@ void Block::Traverse(dGeomID Object){
}
}
bool Block::Inside(const dReal* AABB){
return AABB[AXIS0 * 2 + 0] >= mMinX && AABB[AXIS0 * 2 + 1] < mMaxX && AABB[AXIS1 * 2 + 0] >= mMinZ && AABB[AXIS1 * 2 + 1] < mMaxZ;
bool Block::Inside(const dReal* AABB)
{
return AABB[AXIS0 * 2 + 0] >= mMinX &&
AABB[AXIS0 * 2 + 1] < mMaxX &&
AABB[AXIS1 * 2 + 0] >= mMinZ &&
AABB[AXIS1 * 2 + 1] < mMaxZ;
}
Block* Block::GetBlock(const dReal* AABB){
if (Inside(AABB)){
Block* Block::GetBlock(const dReal* AABB)
{
if (Inside(AABB))
{
return GetBlockChild(AABB); // Child or this will have a good block
}
else if (mParent){
else if (mParent)
{
return mParent->GetBlock(AABB); // Parent has a good block
}
else return this; // We are at the root, so we have little choice
}
Block* Block::GetBlockChild(const dReal* AABB){
if (mChildren){
for (int i = 0; i < SPLITS; i++){
Block* Block::GetBlockChild(const dReal* AABB)
{
if (mChildren)
{
for (int i = 0; i < SPLITS; i++)
{
Block &CurrentChild = mChildren[i];
if (CurrentChild.Inside(AABB)){
if (CurrentChild.Inside(AABB))
{
return CurrentChild.GetBlockChild(AABB); // Child will have good block
}
}
@@ -331,7 +301,9 @@ Block* Block::GetBlockChild(const dReal* AABB){
//****************************************************************************
// quadtree space
struct dxQuadTreeSpace : public dxSpace{
struct dxQuadTreeSpace : public dxSpace
{
size_t BlockCount;
Block* Blocks; // Blocks[0] is the root
dArray<dxGeom*> DirtyList;
@@ -351,49 +323,40 @@ struct dxQuadTreeSpace : public dxSpace{
void collide(void* UserData, dNearCallback* Callback);
void collide2(void* UserData, dxGeom* g1, dNearCallback* Callback);
// Temp data
Block* CurrentBlock; // Only used while enumerating
int* CurrentChild; // Only used while enumerating
int CurrentLevel; // Only used while enumerating
dxGeom* CurrentObject; // Only used while enumerating
// int CurrentIndex;
};
namespace {
inline
size_t numNodes(int depth)
inline size_t numNodes(int depth)
{
// A 4-ary tree has (4^(depth+1) - 1)/3 nodes
// Note: split up into multiple constant expressions for readability
const int k = depth+1;
const size_t fourToNthPlusOne = (size_t)1 << (2*k); // 4^k = 2^(2k)
const int k = depth + 1;
const size_t fourToNthPlusOne = (size_t)1 << (2 * k); // 4^k = 2^(2k)
return (fourToNthPlusOne - 1) / 3;
}
}
dxQuadTreeSpace::dxQuadTreeSpace(dSpaceID _space, const dVector3 Center, const dVector3 Extents, int Depth) : dxSpace(_space){
dxQuadTreeSpace::dxQuadTreeSpace(dSpaceID _space, const dVector3 Center, const dVector3 Extents, int Depth) : dxSpace(_space)
{
type = dQuadTreeSpaceClass;
size_t BlockCount = numNodes(Depth);
BlockCount = numNodes(Depth);
if (BlockCount <= 0)
return;
Blocks = (Block*)dAlloc(BlockCount * sizeof(Block));
Block* Blocks = this->Blocks + 1; // This pointer gets modified!
if (!Blocks)
{
BlockCount = 0;
return;
}
dReal MinX = Center[AXIS0] - Extents[AXIS0];
dReal MaxX = dNextAfter((Center[AXIS0] + Extents[AXIS0]), (dReal)dInfinity);
dReal MinZ = Center[AXIS1] - Extents[AXIS1];
dReal MaxZ = dNextAfter((Center[AXIS1] + Extents[AXIS1]), (dReal)dInfinity);
this->Blocks[0].Create(MinX, MaxX, MinZ, MaxZ, 0, Depth, Blocks);
CurrentBlock = 0;
CurrentChild = (int*)dAlloc((Depth + 1) * sizeof(int));
CurrentLevel = 0;
CurrentObject = 0;
// CurrentIndex = -1;
Block* nBlocks = this->Blocks + 1; // This pointer gets modified!
this->Blocks[0].Create(MinX, MaxX, MinZ, MaxZ, 0, Depth, nBlocks);
// Init AABB. We initialize to infinity because it is not illegal for an object to be outside of the tree. Its simply inserted in the root block
aabb[0] = -dInfinity;
@@ -404,18 +367,12 @@ dxQuadTreeSpace::dxQuadTreeSpace(dSpaceID _space, const dVector3 Center, const d
aabb[5] = dInfinity;
}
dxQuadTreeSpace::~dxQuadTreeSpace(){
int Depth = 0;
Block* Current = &Blocks[0];
while (Current){
Depth++;
Current = Current->mChildren;
dxQuadTreeSpace::~dxQuadTreeSpace()
{
if (Blocks && BlockCount > 0)
{
dFree(Blocks, BlockCount * sizeof(Block));
}
size_t BlockCount = numNodes(Depth);
dFree(Blocks, BlockCount * sizeof(Block));
dFree(CurrentChild, (Depth + 1) * sizeof(int));
}
dxGeom* dxQuadTreeSpace::getGeom(int Index){
@@ -424,8 +381,6 @@ dxGeom* dxQuadTreeSpace::getGeom(int Index){
//@@@
dDebug (0,"dxQuadTreeSpace::getGeom() not yet implemented");
return 0;
// This doesnt work
/*
if (CurrentIndex == Index){
@@ -487,9 +442,8 @@ PARENTRECURSE:
else for (int i = 0; i < Index; i++){ // this will be verrrrrrry slow
getGeom(i);
}
*/
return 0;
*/
}
void dxQuadTreeSpace::add(dxGeom* g){
trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_capsule.cpp
+1 -5
View File
@@ -879,7 +879,6 @@ static void dQueryCCTLPotentialCollisionTriangles(OBBCollider &Collider,
obbRot.m[0][2] = capPtr[8];
obbRot.m[1][2] = capPtr[9];
obbRot.m[2][2] = capPtr[10];
Point cCenter(cData.m_vCapsulePosition[0], cData.m_vCapsulePosition[1], cData.m_vCapsulePosition[2]);
Point cExtents(cData.m_fCapsuleRadius, cData.m_fCapsuleRadius, cData.m_fCapsuleSize);
@@ -888,8 +887,6 @@ static void dQueryCCTLPotentialCollisionTriangles(OBBCollider &Collider,
Matrix4x4 MeshMatrix;
MakeMatrix(cData.m_mTriMeshPos, cData.m_mTriMeshRot, MeshMatrix);
// TC results
if (TriMesh->doBoxTC)
{
@@ -918,9 +915,8 @@ static void dQueryCCTLPotentialCollisionTriangles(OBBCollider &Collider,
else
{
Collider.SetTemporalCoherence(false);
Collider.Collide(BoxCache, obbCapsule, TriMesh->Data->BVTree, null,null /*&MeshMatrix*/);
Collider.Collide(BoxCache, obbCapsule, TriMesh->Data->BVTree, null, &MeshMatrix) ;
}
}
// capsule - trimesh by CroTeam
trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_internal.h
+1 -1
View File
@@ -249,7 +249,7 @@ struct dxTriMesh : public dxGeom
void ClearTCCache();
bool controlGeometry(int controlClass, int controlCode, void *dataValue, int *dataSize);
// bool controlGeometry(int controlClass, int controlCode, void *dataValue, int *dataSize);
void computeAABB();
trunk/unmanaged/ubODE-OpenSim/ode/src/collision_trimesh_opcode.cpp
+5 -5
View File
@@ -80,7 +80,7 @@ dxTriMeshData::Build(const void* Vertices,int VertexCount,
Mesh.SetNbTriangles(IndexCount / 3);
Mesh.SetNbVertices(VertexCount);
Mesh.SetPointers((IndexedTriangle*)Indices, (Point*)Vertices);
Mesh.SetSingle(true);
//Mesh.SetSingle(true);
int vertStride = Mesh.GetVertexStride();
@@ -265,8 +265,8 @@ void dxTriMeshData::Preprocess()
meshFlags = dxTriMeshData::convex | dxTriMeshData::closedSurface;
uint8 notconvex = ~dxTriMeshData::convex;
uint8 notclosed = ~dxTriMeshData::closedSurface;
const uint8 notconvex = (uint8)~dxTriMeshData::convex;
const uint8 notclosed = (uint8)~dxTriMeshData::closedSurface;
EdgeRecord* records = new EdgeRecord[numEdges];
@@ -541,7 +541,7 @@ void dxTriMesh::ClearTCCache()
CapsuleTCCache.setSize(0);
}
/*
bool dxTriMesh::controlGeometry(int controlClass, int controlCode, void *dataValue, int *dataSize)
{
if (controlClass == dGeomColliderControlClass) {
@@ -560,7 +560,7 @@ bool dxTriMesh::controlGeometry(int controlClass, int controlCode, void *dataVal
return dxGeom::controlGeometry(controlClass, controlCode, dataValue, dataSize);
}
*/
bool dxTriMesh::controlGeometry_SetMergeSphereContacts(int dataValue)
{
if (dataValue == dGeomColliderMergeContactsValue__Default) {
trunk/unmanaged/ubODE-OpenSim/ode/src/joints/amotor.cpp
+1 -1
View File
@@ -181,7 +181,7 @@ dxJointAMotor::setEulerReferenceVectors()
void
dxJointAMotor::getSureMaxInfo( SureMaxInfo* info )
{
info->max_m = num;
info->max_m = (uint8)num;
}
trunk/unmanaged/ubODE-OpenSim/ode/src/joints/contact.cpp
+2 -2
View File
@@ -95,8 +95,8 @@ dxJointContact::getInfo1( dxJoint::Info1 *info )
}
the_m = m;
info->m = m;
info->nub = nub;
info->m = (uint8)m;
info->nub = (uint8)nub;
}
trunk/unmanaged/ubODE-OpenSim/ode/src/joints/joint.cpp
+8 -8
View File
@@ -605,7 +605,8 @@ int dxJointLimitMotor::addLimot( dxJoint *joint,
const dVector3 ax1, int rotational )
{
int srow = row * info->rowskip;
dxBody *b0;
dxBody *b1;
// if the joint is powered, or has joint limits, add in the extra row
int powered = fmax > 0;
if ( powered || limit )
@@ -617,7 +618,7 @@ int dxJointLimitMotor::addLimot( dxJoint *joint,
J1[srow+1] = ax1[1];
J1[srow+2] = ax1[2];
dxBody *b1 = joint->node[1].body;
b1 = joint->node[1].body;
if ( b1 )
{
J2[srow+0] = -ax1[0];
@@ -641,7 +642,7 @@ int dxJointLimitMotor::addLimot( dxJoint *joint,
dVector3 ltd = {0,0,0}; // Linear Torque Decoupling vector (a torque)
if ( !rotational && b1 )
{
dxBody *b0 = joint->node[0].body;
b0 = joint->node[0].body;
dVector3 c;
c[0] = REAL( 0.5 ) * ( b1->posr.pos[0] - b0->posr.pos[0] );
c[1] = REAL( 0.5 ) * ( b1->posr.pos[1] - b0->posr.pos[1] );
@@ -688,14 +689,14 @@ int dxJointLimitMotor::addLimot( dxJoint *joint,
dReal fm_ax1_0 = fm*ax1[0], fm_ax1_1 = fm*ax1[1], fm_ax1_2 = fm*ax1[2];
dxBody *b0 = joint->node[0].body;
b0 = joint->node[0].body;
dxWorldProcessContext *world_process_context = b0->world->UnsafeGetWorldProcessingContext();
world_process_context->LockForAddLimotSerialization();
if ( rotational )
{
dxBody *b1 = joint->node[1].body;
b1 = joint->node[1].body;
if ( b1 != NULL )
{
dBodyAddTorque( b1, fm_ax1_0, fm_ax1_1, fm_ax1_2 );
@@ -705,7 +706,7 @@ int dxJointLimitMotor::addLimot( dxJoint *joint,
}
else
{
dxBody *b1 = joint->node[1].body;
b1 = joint->node[1].body;
if ( b1 != NULL )
{
// linear limot torque decoupling step: refer to above discussion
@@ -754,7 +755,6 @@ int dxJointLimitMotor::addLimot( dxJoint *joint,
if ( bounce > 0 )
{
// calculate joint velocity
dReal vel;
if ( rotational )
{
vel = dCalcVectorDot3( joint->node[0].body->avel, ax1 );
@@ -908,7 +908,7 @@ int dxJointLimitMotor::addTwoPointLimot( dxJoint *joint, dReal fps,
if ( bounce > 0 )
{
// calculate relative velocity of the two anchor points
dReal vel =
vel =
dCalcVectorDot3( joint->node[0].body->lvel, &(info->J1l[srow])) +
dCalcVectorDot3( joint->node[0].body->avel, &(info->J1a[srow]));
if (joint->node[1].body) {
trunk/unmanaged/ubODE-OpenSim/ode/src/joints/lmotor.cpp
+1 -1
View File
@@ -69,7 +69,7 @@ dxJointLMotor::computeGlobalAxes( dVector3 ax[3] )
void
dxJointLMotor::getSureMaxInfo( SureMaxInfo* info )
{
info->max_m = num;
info->max_m = (uint8)num;
}
void
trunk/unmanaged/ubODE-OpenSim/ode/src/joints/transmission.cpp
+7 -10
View File
@@ -306,22 +306,19 @@ dxJointTransmission::getInfo2( dReal worldFPS,
// the contact points and tangents across the baseline.
if (mode == dTransmissionChainDrive && delta < 0) {
dVector3 d;
dSubtractVectors3r4(d, a[0], a[1]);
for (i = 0 ; i < 2 ; i += 1) {
dVector3 nn;
dReal a;
dVector3 nnv;
dReal ra;
dCalcVectorCross3(nn, n[i], d);
a = dCalcVectorDot3(nn, nn);
dCalcVectorCross3(nnv, n[i], d);
ra = dCalcVectorDot3(nnv, nnv);
dIASSERT(a > 0);
dAddScaledVectors3r4(c[i], c[i], nn,
1, -2 * dCalcVectorDot3(c[i], nn) / a);
dAddScaledVectors3r4(l[i], l[i], nn,
-1, 2 * dCalcVectorDot3(l[i], nn) / a);
ra = 1 / ra;
dAddScaledVectors3r4(c[i], c[i], nnv, 1, -2 * dCalcVectorDot3(c[i], nnv) * ra);
dAddScaledVectors3r4(l[i], l[i], nnv,-1, 2 * dCalcVectorDot3(l[i], nnv) * ra);
}
}
trunk/unmanaged/ubODE-OpenSim/ode/src/ode.cpp
+7 -7
View File
@@ -180,8 +180,8 @@ static void checkWorld (dxWorld *w)
for (int i=0; i<2; i++) {
if (j->node[i].body) {
int ok = 0;
for (dxJointNode *n=j->node[i].body->firstjoint; n; n=n->next) {
if (n->joint == j) ok = 1;
for (dxJointNode *nj = j->node[i].body->firstjoint; nj; nj = nj->next) {
if (nj->joint == j) ok = 1;
}
if (ok==0) dDebug (0,"joint not in joint list of attached body");
}
@@ -190,16 +190,16 @@ static void checkWorld (dxWorld *w)
// check all body joint lists (correct body ptrs)
for (b=w->firstbody; b; b=(dxBody*)b->next) {
for (dxJointNode *n=b->firstjoint; n; n=n->next) {
if (&n->joint->node[0] == n) {
if (n->joint->node[1].body != b)
for (dxJointNode *nj=b->firstjoint; nj; nj=nj->next) {
if (&nj->joint->node[0] == nj) {
if (nj->joint->node[1].body != b)
dDebug (0,"bad body pointer in joint node of body list (1)");
}
else {
if (n->joint->node[0].body != b)
if (nj->joint->node[0].body != b)
dDebug (0,"bad body pointer in joint node of body list (2)");
}
if (n->joint->tag != count) dDebug (0,"bad joint node pointer in body");
if (nj->joint->tag != count) dDebug (0,"bad joint node pointer in body");
}
}