The object does not continue its trajectory after deactivation of constraint about physx HOT 12 CLOSED

#define PX_PHYSX_STATIC_LIB
#include <iostream>

#include <ctype.h>
#include <cmath>
#include <algorithm>
#include <vector>

#include "PxPhysicsAPI.h"
#include "foundation/PxPreprocessor.h"

#define PVD_HOST "127.0.0.1"
#define PX_RELEASE(x)   if(x)   { x->release(); x = NULL;   }

using namespace physx;

PxDefaultAllocator      gAllocator;
PxDefaultErrorCallback  gErrorCallback;

PxFoundation* gFoundation = NULL;
PxPhysics* gPhysics = NULL;

PxDefaultCpuDispatcher* gDispatcher = NULL;
PxScene* gScene = NULL;

PxMaterial* gMaterial = NULL;

PxPvd* gPvd = NULL;

PxD6Joint* joint = NULL;
PxRigidDynamic* parent = NULL;



enum InterpolationType
{
    LINEAR,
    STEP
};

template <typename T>
struct KeyFrame
{
    float time;
    T value;
    InterpolationType interpType;
};

template <typename T>
class AnimCurve
{
public:
    AnimCurve() {}

    template <typename U>
    void AddKeyframe(float time, U value, InterpolationType interpType = LINEAR)
    {
        KeyFrame<U> key;
        key.time = time;
        key.value = value;
        key.interpType = interpType;
        animationKeys.push_back(key);
    }

    template <typename U>
    void AddKey(float time, U value, InterpolationType interpType = LINEAR)
    {
        AddKeyframe(time, value, interpType);
    }

    template <typename U>
    U Interpolate(float t, const KeyFrame<U>& key1, const KeyFrame<U>& key2)
    {
        if (key1.interpType == STEP || key2.interpType == STEP)
        {
            return U(key1.value);
        }
        else if (t <= key1.time)
        {
            return U(key1.value);
        }
        else if (t >= key2.time)
        {
            return U(key2.value);
        }
        else
        {
            float alpha = (t - key1.time) / (key2.time - key1.time);
            return U(key1.value + alpha * (key2.value - key1.value));
        }
    }

    template <typename U>
    U GetValue(int frame)
    {
        float currentTime = static_cast<float>(frame);

        if (animationKeys.empty())
        {
            return U(); 
        }

        if (frame <= 0 && currentTime <= animationKeys.front().time)
        {
            return U(animationKeys.front().value);
        }
        else if (frame >= 0 && currentTime >= animationKeys.back().time)
        {
            return U(animationKeys.back().value); 
        }

        KeyFrame<U> key1, key2;
        for (const KeyFrame<U>& key : animationKeys)
        {
            if (key.time <= currentTime)
            {
                key1 = key;
            }
            else
            {
                key2 = key;
                break;
            }
        }

        return static_cast<U>(Interpolate(currentTime, key1, key2));
    }

    template <typename U>
    int FindKeyframeIndex(float time)
    {
        for (size_t i = 0; i < animationKeys.size(); i++)
        {
            if (animationKeys[i].time == time)
            {
                return static_cast<int>(i);
            }
        }
        return -1;
    }

    template <typename U>
    float FindTimeFromIndex(int index)
    {
        if (index >= 0 && static_cast<size_t>(index) < animationKeys.size())
        {
            return animationKeys[index].time;
        }
        return -1.0f;
    }

    template <typename U>
    bool SetValue(int index, U value)
    {
        float time = FindTimeFromIndex(index);
        if (time != -1.0f)
        {
            animationKeys[index].value = value;
            return true;
        }
        return false;
    }

private:
    std::vector<KeyFrame<T>> animationKeys;
};

AnimCurve<float> rigid_translateX;
AnimCurve<bool> joint_enabled;

void createD6(PxRigidActor* parent, const PxTransform& parentTm,
    PxRigidActor* child, const PxTransform& childTm, PxD6Joint*& j)
{
    j = PxD6JointCreate(*gPhysics, parent, parentTm, child, childTm);

    j->setMotion(PxD6Axis::eX, PxD6Motion::eLIMITED);
    j->setMotion(PxD6Axis::eY, PxD6Motion::eLIMITED);
    j->setMotion(PxD6Axis::eZ, PxD6Motion::eLIMITED);

    const PxSpring linearSpring{ 10000000.f, 1000000.f };
    const PxJointLinearLimitPair linearLimit{ -0.01f, 0.01f, linearSpring };

    j->setLinearLimit(PxD6Axis::eX, linearLimit);
    j->setLinearLimit(PxD6Axis::eY, linearLimit);
    j->setLinearLimit(PxD6Axis::eZ, linearLimit);


    j->setMotion(PxD6Axis::eSWING1, PxD6Motion::eLIMITED);
    j->setMotion(PxD6Axis::eSWING2, PxD6Motion::eLIMITED);
    j->setMotion(PxD6Axis::eTWIST, PxD6Motion::eLIMITED);
    const PxJointLimitCone limitCone(0.00001f, 0.00001f, linearSpring);
    j->setSwingLimit(limitCone);
    const PxJointAngularLimitPair limitSwing(-0.00001f, 0.00001f, linearSpring);
    j->setTwistLimit(limitSwing);
}



void initPhysics() 
{
    gFoundation = PxCreateFoundation(PX_PHYSICS_VERSION, gAllocator, gErrorCallback);
    gPvd = PxCreatePvd(*gFoundation);
    PxPvdTransport* transport = PxDefaultPvdSocketTransportCreate(PVD_HOST, 5425, 10);
    gPvd->connect(*transport, PxPvdInstrumentationFlag::eALL);

    gPhysics = PxCreatePhysics(PX_PHYSICS_VERSION, *gFoundation, PxTolerancesScale(), true, gPvd);
    PxInitExtensions(*gPhysics, gPvd);

    PxSceneDesc sceneDesc(gPhysics->getTolerancesScale());
    sceneDesc.gravity = PxVec3(0.0f, -98.0f, 0.0f);
    gDispatcher = PxDefaultCpuDispatcherCreate(2);
    sceneDesc.cpuDispatcher = gDispatcher;
    sceneDesc.filterShader = PxDefaultSimulationFilterShader;
    sceneDesc.solverType = PxSolverType::eTGS;

    gScene = gPhysics->createScene(sceneDesc);

    PxPvdSceneClient* pvdClient = gScene->getScenePvdClient();
    if (pvdClient) {
        pvdClient->setScenePvdFlag(PxPvdSceneFlag::eTRANSMIT_CONSTRAINTS, true);
        pvdClient->setScenePvdFlag(PxPvdSceneFlag::eTRANSMIT_CONTACTS, true);
        pvdClient->setScenePvdFlag(PxPvdSceneFlag::eTRANSMIT_SCENEQUERIES, true);
    }
}


void createBouncingBall()
{
    gMaterial = gPhysics->createMaterial(0.8f, 0.8f, 0.8f);
    PxRigidStatic* groundPlane = PxCreatePlane(*gPhysics, PxPlane(0, 1, 0, 0), *gMaterial);
    gScene->addActor(*groundPlane);

    PxTransform tm{ PxVec3(0.0f, 20.0f, -20.0f) };
    PxSphereGeometry geo{ 1.f };
    parent = PxCreateDynamic(*gPhysics, tm, geo, *gMaterial, 100.0f);
    parent->setRigidBodyFlag(PxRigidBodyFlag::eKINEMATIC, true);
    gScene->addActor(*parent);

    //joint creation
    createD6(NULL, PxTransform(PxIdentity), parent, PxTransform(PxIdentity), joint);

}


void stepPhysics(int frame)
{
    float translateX = rigid_translateX.GetValue<float>(frame);
    bool enabled = joint_enabled.GetValue<bool>(frame);

    PxTransform tm{ PxVec3(translateX, 20.0f, -20.0f) };
    parent->setKinematicTarget(tm);

    joint->setConstraintFlag(PxConstraintFlag::eDISABLE_CONSTRAINT, !enabled);
    parent->setRigidBodyFlag(PxRigidBodyFlag::eKINEMATIC, enabled);

    //joint->setLocalPose(PxJointActorIndex::eACTOR0, tm);

    gScene->simulate(1.0f / 60.f);
    gScene->fetchResults(true);
}


void cleanupPhysics() {
    PX_RELEASE(gScene);
    PX_RELEASE(gDispatcher);
    PxCloseExtensions();
    PX_RELEASE(gPhysics);
    if (gPvd) {
        PxPvdTransport* transport = gPvd->getTransport();
        gPvd->release();    gPvd = NULL;
        PX_RELEASE(transport);
    }
    PX_RELEASE(gFoundation);

}


int main(int, const char* const*) {
    static const int frameCount = 500;

    rigid_translateX.AddKey(0, 0.0f);
    rigid_translateX.AddKey(30, 20.0f);


    joint_enabled.AddKey(0, true, InterpolationType::STEP);
    joint_enabled.AddKey(5, false, InterpolationType::STEP);

    initPhysics();
    createBouncingBall();

    for (int i = 0; i < frameCount; i++)
    {
        stepPhysics(i);
    }

    cleanupPhysics();

    return 0;
}