Esempio n. 1
0
void Sc::BodySim::calculateKinematicVelocity(PxReal oneOverDt)
{
    PX_ASSERT(isKinematic());

    /*------------------------------------------------\
    | kinematic bodies are moved directly by the user and are not influenced by external forces
    | we simply determine the distance moved since the last simulation frame and
    | assign the appropriate delta to the velocity. This vel will be used to shove dynamic
    | objects in the solver.
    | We have to do this like so in a delayed way, because when the user sets the target pos the dt is not
    | yet known.
    \------------------------------------------------*/
    PX_ASSERT(isActive());

    BodyCore& core = getBodyCore();

    if (readInternalFlag(BF_KINEMATIC_MOVED))
    {
        clearInternalFlag(BF_KINEMATIC_SETTLING);
        const SimStateData* kData = core.getSimStateData(true);
        PX_ASSERT(kData);
        PX_ASSERT(kData->isKine());
        PX_ASSERT(kData->getKinematicData()->targetValid);
        PxVec3 linVelLL, angVelLL;
        PxTransform targetPose = kData->getKinematicData()->targetPose;
        const PxTransform& currBody2World = getBody2World();

        //the kinematic target pose is now the target of the body (CoM) and not the actor.

        PxVec3 deltaPos = targetPose.p;
        deltaPos -= currBody2World.p;
        linVelLL = deltaPos * oneOverDt;

        PxQuat q = targetPose.q * currBody2World.q.getConjugate();

        if (q.w < 0)	//shortest angle.
            q = -q;

        PxReal angle;
        PxVec3 axis;
        q.toRadiansAndUnitAxis(angle, axis);
        angVelLL = axis * angle * oneOverDt;

        core.setLinearVelocity(linVelLL);
        core.setAngularVelocity(angVelLL);

        // Moving a kinematic should trigger a wakeUp call on a higher level.
        PX_ASSERT(core.getWakeCounter()>0);
        PX_ASSERT(isActive());

    }
    else
    {
        core.setLinearVelocity(PxVec3(0));
        core.setAngularVelocity(PxVec3(0));
    }
}
/**
 @brief MouseRButtonUp
 @date 2014-03-04
*/
void COrientationEditController::MouseRButtonUp(physx::PxU32 x, physx::PxU32 y)
{
	switch (m_editMode)
	{
	case MODE_POSITION:
		{
			RET(!m_selectNode);

			using namespace genotype_parser;

			SConnection *joint = m_rootNode->GetJoint(m_selectNode);
			RET(!joint);

			PxVec3 dir = m_selectNode->GetPos() - m_rootNode->GetPos();
			const float len = dir.magnitude();
			dir.normalize();

			PxQuat rotateToXAxis;
			if (boost::iequals(joint->type, "revolute"))
			{
				PxVec3 jointAxis = PxVec3(0,0,1).cross(dir);
				jointAxis.normalize();
				utility::quatRotationArc(rotateToXAxis, PxVec3(1,0,0), jointAxis);
			}
			else
			{
				rotateToXAxis = m_rootNode->GetWorldTransform().q;
			}

			{ // setting parent orientation
				PxReal angle;
				PxVec3 axis;
				rotateToXAxis.toRadiansAndUnitAxis(angle, axis);
				joint->parentOrient.angle = angle;
				joint->parentOrient.dir = utility::PxVec3toVec3(axis);
				 
				printf( "parent angle = %f, dir= %f %f %f\n", angle, axis.x, axis.y, axis.z);
			}
			
			{ // setting select orientation
				PxVec3 pos;
				PxTransform rotTm;
				const PxTransform tm = m_selectNode->GetWorldTransform();
				if (boost::iequals(joint->type, "revolute"))
				{
					rotTm = PxTransform(rotateToXAxis) * PxTransform(tm.q);
					pos = rotTm.rotate(dir*len);
				}
				else
				{
					rotTm = PxTransform(tm.q);
					pos = tm.p;
				}

				PxReal angle;
				PxVec3 axis;
				rotTm.q.toRadiansAndUnitAxis(angle, axis);
				joint->orient.angle = angle;
				joint->orient.dir = utility::PxVec3toVec3(axis);
				joint->pos = utility::PxVec3toVec3(pos);

				printf( "connect angle = %f, dir= %f %f %f\n", angle, axis.x, axis.y, axis.z);
			}

			SelectNode(NULL);
		}
		break;
	}

	if (m_cursor)
		m_cursor->MouseRButtonUp(x,y);
}