Inconsistent box on box penetration when using SOLVER_USE_2_FRICTION_DIRECTIONS and CF_CUSTOM_MATERIAL_CALLBACK.

[bhelyer]

Hi there, I've been using CF_CUSTOM_MATERIAL_CALLBACK to simulate a conveyor like motion. However, when the box on the 'conveyor' comes to a complete stop (m_contactMotion1 == 0), I've found that the box would sometimes refuse to start moving again when m_contactMotion1 is restored to a non-zero value.

Initially I thought that the box or the conveyor was deactivating, but actually the speed was being gathered very very slowly, and the box will eventually start moving if left for long enough.

I've figured out that if SOLVER_USE_2_FRICTION_DIRECTIONS is enabled, sometimes (it seems to happen far more often if btScalar is a double) only two points are detected as penetrating. The reason is that in btBoxBoxDetector.cpp:648, the calculated penetration depth is very slightly (~-0.000001) below zero, so the collision point is discarded and the custom material callback only sees two collision points instead of the usual four.

I've modified the BasicExample to show the issue. Three seconds after stopping, the box should start to move, but it doesn't. (If it does, try again, and try with btScalars as doubles. In the code this was adapted from, it's fairly sporadic.) I can workaround the issue by changing the aforemention comparison in btBoxBoxDetector to allow numbers slightly below zero, but I'm hoping it's something obvious I'm doing wrong!

/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2015 Google Inc. http://bulletphysics.org

This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose, 
including commercial applications, and to alter it and redistribute it freely, 
subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/

#include "BasicExample.h"

#include "btBulletDynamicsCommon.h"
#define ARRAY_SIZE_Y 5
#define ARRAY_SIZE_X 5
#define ARRAY_SIZE_Z 5

#include "LinearMath/btVector3.h"
#include "LinearMath/btAlignedObjectArray.h"

#include "../CommonInterfaces/CommonRigidBodyBase.h"

#include <chrono>
using namespace std::chrono;

struct BasicExample : public CommonRigidBodyBase
{
	high_resolution_clock::time_point m_last_state_change;
	bool m_leave_conveyor_on = false;
	btRigidBody* m_conveyor1 = nullptr;
	btRigidBody* m_conveyor2 = nullptr;
	btRigidBody* m_box = nullptr;

	BasicExample(struct GUIHelperInterface* helper)
		: CommonRigidBodyBase(helper)
	{
	}
	virtual ~BasicExample() {}
	virtual void initPhysics();
	virtual void renderScene();
	void resetCamera()
	{
		float dist = 4;
		float pitch = -35;
		float yaw = 52;
		float targetPos[3] = {0, 0, 0};
		m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
	}
};

btScalar g_current_speed = 1;

static bool onMaterialCollision(btManifoldPoint& cp,
    const btCollisionObjectWrapper* obj0, int partId0, int index0,
    const btCollisionObjectWrapper* obj1, int partId1, int index1)
{
	cp.m_normalWorldOnB = btVector3(0, -1, 0);
	cp.m_contactPointFlags = BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED;
	cp.m_lateralFrictionDir1.setValue(0, 0, -1);
	cp.m_lateralFrictionDir2 = btScalar(1) * cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
	cp.m_contactMotion1 = g_current_speed;
	return true;
}

void BasicExample::initPhysics()
{

	m_guiHelper->setUpAxis(1);

	createEmptyDynamicsWorld();
	m_dynamicsWorld->getSolverInfo().m_solverMode |= SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION;
	m_dynamicsWorld->getSolverInfo().m_solverMode |= SOLVER_ENABLE_FRICTION_DIRECTION_CACHING;

    // Removing this fixes the issue, but the box turns at an angle as it starts moving again:
	m_dynamicsWorld->getSolverInfo().m_solverMode |= SOLVER_USE_2_FRICTION_DIRECTIONS;

	gContactAddedCallback = onMaterialCollision;

	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);

	if (m_dynamicsWorld->getDebugDrawer())
		m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe + btIDebugDraw::DBG_DrawContactPoints);

    const auto conveyorHalfExtents = btVector3(btScalar(0.381), btScalar(0.100584), btScalar(0.4572));
    btBoxShape* conveyorShape = createBoxShape(conveyorHalfExtents);

    btTransform conveyorTransform;
	conveyorTransform.setIdentity();
	btScalar zeroMass(0);
	m_conveyor1 = createRigidBody(zeroMass, conveyorTransform, conveyorShape);
	m_conveyor1->setCollisionFlags(m_conveyor1->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
	m_conveyor1->setActivationState(DISABLE_DEACTIVATION);

    conveyorTransform.getOrigin() += btVector3(0, 0, conveyorHalfExtents.z() * 2);
	m_conveyor2 = createRigidBody(zeroMass, conveyorTransform, conveyorShape);
	m_conveyor2->setCollisionFlags(m_conveyor2->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
	m_conveyor2->setActivationState(DISABLE_DEACTIVATION);

    const auto boxHalfExtents = btVector3(btScalar(0.1524), btScalar(0.0762), btScalar(0.1524));
	btBoxShape* boxShape = createBoxShape(boxHalfExtents);

    btTransform boxTransform;
	boxTransform.setIdentity();
	boxTransform.getOrigin().setZ(-(conveyorHalfExtents.z() * 0.8));
	boxTransform.getOrigin().setY(conveyorHalfExtents.y() * 2);
	btScalar mass(1.0);
	m_box = createRigidBody(mass, boxTransform, boxShape, btVector4(1.0, 0.647, 0.0, 1.0));
	m_box->setActivationState(DISABLE_DEACTIVATION);

	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);

    m_last_state_change = high_resolution_clock::now();
}

void BasicExample::renderScene()
{
	CommonRigidBodyBase::renderScene();

    const auto now = high_resolution_clock::now();
    // After half a second, turn 'off' the conveyor.
	if (!m_leave_conveyor_on && g_current_speed > 0.5 && now - m_last_state_change >= 0.5s)
    {
		g_current_speed = 0;
		m_last_state_change = now;
    }
    // Then after three seconds, turn it back 'on' again.
	else if (g_current_speed < 0.5 && now - m_last_state_change >= 3.0s)
    {
		g_current_speed = 1;
		m_last_state_change = now;
        // Uncomment this line to see the box 'fix' itself:
		//m_leave_conveyor_on = true;
    }
}

CommonExampleInterface* BasicExampleCreateFunc(CommonExampleOptions& options)
{
	return new BasicExample(options.m_guiHelper);
}

B3_STANDALONE_EXAMPLE(BasicExampleCreateFunc)