diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
index c2a23dfb2..24db9567e 100644
--- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
+++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
@@ -738,23 +738,21 @@ int	btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
 {
 #if BT_THREADSAFE
     int solverBodyId = -1;
-    if ( !body.isStaticOrKinematicObject() )
+    bool isRigidBodyType = btRigidBody::upcast( &body ) != NULL;
+    if ( isRigidBodyType && !body.isStaticOrKinematicObject() )
     {
         // dynamic body
         // Dynamic bodies can only be in one island, so it's safe to write to the companionId
         solverBodyId = body.getCompanionId();
         if ( solverBodyId < 0 )
         {
-            if ( btRigidBody* rb = btRigidBody::upcast( &body ) )
-            {
-                solverBodyId = m_tmpSolverBodyPool.size();
-                btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
-                initSolverBody( &solverBody, &body, timeStep );
-                body.setCompanionId( solverBodyId );
-            }
+            solverBodyId = m_tmpSolverBodyPool.size();
+            btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
+            initSolverBody( &solverBody, &body, timeStep );
+            body.setCompanionId( solverBodyId );
         }
     }
-    else if (body.isKinematicObject())
+    else if (isRigidBodyType && body.isKinematicObject())
     {
         //
         // NOTE: must test for kinematic before static because some kinematic objects also
@@ -774,7 +772,6 @@ int	btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
         if ( solverBodyId == INVALID_SOLVER_BODY_ID )
         {
             // create a table entry for this body
-            btRigidBody* rb = btRigidBody::upcast( &body );
             solverBodyId = m_tmpSolverBodyPool.size();
             btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
             initSolverBody( &solverBody, &body, timeStep );
@@ -792,7 +789,7 @@ int	btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
         }
         solverBodyId = m_fixedBodyId;
     }
-    btAssert( solverBodyId < m_tmpSolverBodyPool.size() );
+    btAssert( solverBodyId >= 0 && solverBodyId < m_tmpSolverBodyPool.size() );
 	return solverBodyId;
 #else // BT_THREADSAFE
 
diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp
index 82a719a3e..6fe068aac 100644
--- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp
+++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp
@@ -317,7 +317,8 @@ int	btSequentialImpulseConstraintSolverMt::getOrInitSolverBodyThreadsafe(btColli
     // getOrInitSolverBodyThreadsafe -- attempts to be fully threadsafe (however may affect determinism)
     //
     int solverBodyId = -1;
-    if ( !body.isStaticOrKinematicObject() )
+    bool isRigidBodyType = btRigidBody::upcast( &body ) != NULL;
+    if ( isRigidBodyType && !body.isStaticOrKinematicObject() )
     {
         // dynamic body
         // Dynamic bodies can only be in one island, so it's safe to write to the companionId
@@ -329,18 +330,15 @@ int	btSequentialImpulseConstraintSolverMt::getOrInitSolverBodyThreadsafe(btColli
             solverBodyId = body.getCompanionId();
             if ( solverBodyId < 0 )
             {
-                if ( btRigidBody* rb = btRigidBody::upcast( &body ) )
-                {
-                    solverBodyId = m_tmpSolverBodyPool.size();
-                    btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
-                    initSolverBody( &solverBody, &body, timeStep );
-                    body.setCompanionId( solverBodyId );
-                }
+                solverBodyId = m_tmpSolverBodyPool.size();
+                btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
+                initSolverBody( &solverBody, &body, timeStep );
+                body.setCompanionId( solverBodyId );
             }
             m_bodySolverArrayMutex.unlock();
         }
     }
-    else if (body.isKinematicObject())
+    else if (isRigidBodyType && body.isKinematicObject())
     {
         //
         // NOTE: must test for kinematic before static because some kinematic objects also
@@ -373,7 +371,6 @@ int	btSequentialImpulseConstraintSolverMt::getOrInitSolverBodyThreadsafe(btColli
             if ( INVALID_SOLVER_BODY_ID == solverBodyId )
             {
                 // create a table entry for this body
-                btRigidBody* rb = btRigidBody::upcast( &body );
                 solverBodyId = m_tmpSolverBodyPool.size();
                 btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
                 initSolverBody( &solverBody, &body, timeStep );
@@ -400,7 +397,7 @@ int	btSequentialImpulseConstraintSolverMt::getOrInitSolverBodyThreadsafe(btColli
         }
         solverBodyId = m_fixedBodyId;
     }
-    btAssert( solverBodyId < m_tmpSolverBodyPool.size() );
+    btAssert( solverBodyId >= 0 && solverBodyId < m_tmpSolverBodyPool.size() );
 	return solverBodyId;
 }