PhysicsJoint.c

Go to the documentation of this file.

/****************************************************************************************/
/*  PHYSICSJOINT.H                                                                      */
/*                                                                                      */
/*  Author: Jason Wood                                                                  */
/*  Description: Rigid body joint implementation                                        */
/*                                                                                      */
/*  The contents of this file are subject to the Genesis3D Public License               */
/*  Version 1.01 (the "License"); you may not use this file except in                   */
/*  compliance with the License. You may obtain a copy of the License at                */
/*  http://www.genesis3d.com                                                            */
/*                                                                                      */
/*  Software distributed under the License is distributed on an "AS IS"                 */
/*  basis, WITHOUT WARRANTY OF ANY KIND, either express or implied.  See                */
/*  the License for the specific language governing rights and limitations              */
/*  under the License.                                                                  */
/*                                                                                      */
/*  The Original Code is Genesis3D, released March 25, 1999.                            */
/*Genesis3D Version 1.1 released November 15, 1999                            */
/*  Copyright (C) 1999 WildTangent, Inc. All Rights Reserved           */
/*                                                                                      */
/****************************************************************************************/
#include <math.h>
#include <assert.h>

#include "vec3d.h"
#include "xform3d.h"
#include "ram.h"
#include "matrix33.h"
#include "quatern.h"

#include "PhysicsObject.h"
#include "PhysicsJoint.h"

typedef struct gePhysicsJoint
{
        gePhysicsJoint_Kind     Type;
        geVec3d                         locationA;
        geVec3d                         locationB;
        geVec3d                         locationAInWorldSpace;
        geVec3d                         locationBInWorldSpace;
        gePhysicsObject *               Object1;
        gePhysicsObject *               Object2;
        geFloat                         assemblyRate;
}       gePhysicsJoint;

#define JOINT_ASSEMBLY_RATE_MULTIPLIER (4.f)

GENESISAPI gePhysicsJoint * GENESISCC gePhysicsJoint_Create(gePhysicsJoint_Kind Kind, const geVec3d *Location, 
        geFloat assemblyRate, gePhysicsObject *PS1, gePhysicsObject *PS2, geFloat physicsScale)
{
        gePhysicsJoint* pPhysjnt;
        geVec3d         POLocation;
        geVec3d         physicsSpaceLocation;

        pPhysjnt = NULL;
        pPhysjnt = GE_RAM_ALLOCATE_STRUCT(gePhysicsJoint);
        if (pPhysjnt == NULL)
        {
                return NULL;
        }

        // make sure the joint makes sense

        pPhysjnt->Type = Kind;
        pPhysjnt->assemblyRate = assemblyRate * JOINT_ASSEMBLY_RATE_MULTIPLIER;
        pPhysjnt->Object1 = PS1;
        pPhysjnt->Object2 = PS2;

        geVec3d_Scale(Location, physicsScale, &physicsSpaceLocation);

        switch (Kind)
        {
                case JT_WORLD:
                        if (PS1 == NULL)
                        {
                                /*
                                GenVSI_Error(VSI, 
                                        GE_FALSE, 
                                        "Joint_Spawn: World joint needs non-NULL gePhysicsObject1 field.\n");
                                */
                                return NULL;
                        }
                        #if 0
                        if (pJoint->Next == pJoint)
                        {
                                /*
                                GenVSI_Error(VSI, 
                                        GE_FALSE, 
                                        "Joint_Spawn: Next field points to parent.\n");
                                */
                                return NULL;
                        }
                        #endif
                        
                        gePhysicsObject_GetLocation(PS1, &POLocation, 0);

                        geVec3d_Subtract(&physicsSpaceLocation,
                                &POLocation,
                                &pPhysjnt->locationA);
                        geVec3d_Copy(&physicsSpaceLocation, &pPhysjnt->locationB);
                        break;

                case JT_SPHERICAL:
                        if (PS1 == NULL || PS2 == NULL)
                        {
                                /*
                                GenVSI_Error(VSI, 
                                        GE_FALSE, 
                                        "Joint_Spawn: Spherical joint needs 2 non-NULL gePhysicsObjects.\n");
                                */
                                return NULL;
                        }
                        #if 0
                        if (pJoint->Next == pJoint)
                        {
                                /*
                                GenVSI_Error(VSI, 
                                        GE_FALSE, 
                                        "Joint_Spawn: Next field points to parent.\n");
                                */
                        }
                        #endif
                        if (PS1 == PS2)
                        {
                                /*
                                GenVSI_Error(VSI, 
                                        GE_FALSE, 
                                        "Joint_Spawn: Spherical joint: need 2 distinct gePhysicsObjects.\n");
                                */
                                return NULL;
                        }

                        gePhysicsObject_GetLocation(PS1, &POLocation, 0);
                        geVec3d_Subtract(&physicsSpaceLocation,
                                &POLocation,
                                &pPhysjnt->locationA);

                        gePhysicsObject_GetLocation(PS2, &POLocation, 0);
                        geVec3d_Subtract(&physicsSpaceLocation,
                                &POLocation,
                                &pPhysjnt->locationB);
                        break;

                default:
                        /*
                        GenVSI_Error(VSI, 
                                GE_FALSE, 
                                "Joint_Spawn: unsupported joint type %d.\n", ij->jointType);
                        */
                        return NULL;
        }

        return pPhysjnt;
}

GENESISAPI geBoolean GENESISCC gePhysicsJoint_Destroy(gePhysicsJoint** ppPhysjnt)
{
        assert(ppPhysjnt != NULL);
        assert(*ppPhysjnt != NULL);

        geRam_Free(*ppPhysjnt);

        *ppPhysjnt = NULL;

        return GE_TRUE;
}

GENESISAPI gePhysicsJoint_Kind GENESISCC gePhysicsJoint_GetType(const gePhysicsJoint* pPhysjnt)
{
        assert(pPhysjnt != NULL);

        return pPhysjnt->Type;
}

GENESISAPI void GENESISCC gePhysicsJoint_GetLocationA(const gePhysicsJoint* pPhysjnt, geVec3d* pLoc)
{
        assert(pPhysjnt != NULL);
        assert(pLoc != NULL);
        
        geVec3d_Copy(&pPhysjnt->locationA, pLoc);
}

GENESISAPI void GENESISCC gePhysicsJoint_GetLocationB(const gePhysicsJoint* pPhysjnt, geVec3d* pLoc)
{
        assert(pPhysjnt != NULL);
        assert(pLoc != NULL);
        
        geVec3d_Copy(&pPhysjnt->locationB, pLoc);
}

GENESISAPI void GENESISCC gePhysicsJoint_SetLocationA(gePhysicsJoint* pPhysjnt, const geVec3d* pLoc)
{
        assert(pPhysjnt != NULL);
        assert(pLoc != NULL);
        
        geVec3d_Copy(pLoc, &pPhysjnt->locationA);
}

GENESISAPI void GENESISCC gePhysicsJoint_SetLocationB(gePhysicsJoint* pPhysjnt, const geVec3d* pLoc)
{
        assert(pPhysjnt != NULL);
        assert(pLoc != NULL);
        
        geVec3d_Copy(pLoc, &pPhysjnt->locationB);
}

GENESISAPI void GENESISCC gePhysicsJoint_GetLocationAInWorldSpace(const gePhysicsJoint* pPhysjnt, geVec3d* pLoc)
{
        assert(pPhysjnt != NULL);
        assert(pLoc != NULL);
        
        geVec3d_Copy(&pPhysjnt->locationAInWorldSpace, pLoc);
}

GENESISAPI void GENESISCC gePhysicsJoint_GetLocationBInWorldSpace(const gePhysicsJoint* pPhysjnt, geVec3d* pLoc)
{
        assert(pPhysjnt != NULL);
        assert(pLoc != NULL);
        
        geVec3d_Copy(&pPhysjnt->locationBInWorldSpace, pLoc);
}

GENESISAPI void GENESISCC gePhysicsJoint_SetLocationAInWorldSpace(gePhysicsJoint* pPhysjnt, const geVec3d* pLoc)
{
        assert(pPhysjnt != NULL);
        assert(pLoc != NULL);
        
        geVec3d_Copy(pLoc, &pPhysjnt->locationAInWorldSpace);
}

GENESISAPI void GENESISCC gePhysicsJoint_SetLocationBInWorldSpace(gePhysicsJoint* pPhysjnt, const geVec3d* pLoc)
{
        assert(pPhysjnt != NULL);
        assert(pLoc != NULL);
        
        geVec3d_Copy(pLoc, &pPhysjnt->locationBInWorldSpace);
}

GENESISAPI gePhysicsObject* GENESISCC gePhysicsJoint_GetObject1(const gePhysicsJoint* pPhysjnt)
{
        assert(pPhysjnt != NULL);

        return pPhysjnt->Object1;
}

GENESISAPI gePhysicsObject* GENESISCC gePhysicsJoint_GetObject2(const gePhysicsJoint* pPhysjnt)
{
        assert(pPhysjnt != NULL);

        return pPhysjnt->Object2;
}

GENESISAPI geFloat GENESISCC gePhysicsJoint_GetAssemblyRate(const gePhysicsJoint* pPhysjnt)
{
        assert(pPhysjnt != NULL);

        return pPhysjnt->assemblyRate;
}

GENESISAPI void GENESISCC gePhysicsJoint_SetAssemblyRate(gePhysicsJoint* pPhysjnt, geFloat assemblyRate)
{
        assert(pPhysjnt != NULL);
        assert(assemblyRate >= (geFloat)(1e-5));

        pPhysjnt->assemblyRate = assemblyRate;
}

---