quatf NAMESPACE::Trackball::deltaRotation(float dx, float dy)
{
//quatf qx=quatf(V3F(1,0,0),dy);
//quatf qy=quatf(qx.inverse()*V3F(0,1,0),dx);
if (dx*dx + dy*dy > 1e-10f) {
quatf qd = quatf(V3F(dy, dx, 0), sqrt(dx*dx + dy*dy));
return (qd);
}
else {
return quatf();
}
}
void NAMESPACE::Trackball::updateRotation(uint x, uint y)
{
float speed = 3.0f; // *m_Elapsed;
float dx = (float(x) - float(m_PrevX)) * speed / float(m_Width);
float dy = (float(y) - float(m_PrevY)) * speed / float(m_Height);
m_PrevX = x;
m_PrevY = y;
quatf dr = deltaRotation(dx, dy);
m_Rotation = dr * m_Rotation;
if (m_Walkthrough) {
// remove 'roll'
int d = -1;
if (m_Up == X_neg || m_Up == Y_neg || m_Up == Z_neg) {
d = 1;
}
quatf rinv = m_Rotation.inverse();
v3f up = dir2v3f(m_Up);
v3f realleft = rinv * V3F(1, 0, 0);
v3f frwd = rinv * V3F(0, 0, 1);
v3f flat = normalize_safe(realleft - dot(realleft, up)*up);
float cs = dot(realleft, flat);
if (cs > -1.0f && cs < 1.0f) {
float sign = dot(up, realleft) > 0 ? -1.0f : 1.0f;
float target_agl = sign * acos(cs);
float agl = target_agl;
m_Rotation = quatf(V3F(0, 0, 1), agl) * m_Rotation;
}
}
}
void bt_soft_body::get_transform(vec3f &position, quatf &rotation) const
{
const btTransform& btxform = m_body->getWorldTransform();
btQuaternion q = btxform.getRotation();
btVector3 p = btxform.getOrigin();
position = vec3f(p.x(), p.y(), p.z());
rotation = quatf(q.w(), q.x(), q.y(), q.z());
}
void NAMESPACE::Trackball::init(uint w, uint h)
{
m_Width = w;
m_Height = h;
m_Translation = V3F(0, 0, -m_Radius);
m_Rotation = quatf(V3F(0, 0, 1), 0.0f);
m_Center = V3F(0, 0, 0);
}
int main ()
{
printf ("Results of quat03_test:\n");
quatf q1 (1.0f, 2.0f, 3.0f, 4.0f), q2 (5.0f, 6.0f, 7.0f, 8.0f);
dump ("q1+q2", q1+q2);
dump ("q1-q2", q1-q2);
dump ("q1*q2", q1*q2);
dump ("q1*2", q1*2.0f);
dump ("q1/2", q1/2.0f);
dump ("2*q1", 2.0f*q1);
dump ("q1+=q2", quatf (q1)+=q2);
dump ("q1-=q2", quatf (q1)-=q2);
dump ("q1*=q2", quatf (q1)*=q2);
dump ("q1*=2", quatf (q1)*=2.0f);
dump ("q1/=2", quatf (q1)/=2.0f);
return 0;
}
NAMESPACE::Trackball::Trackball()
{
m_Rotation = quatf(LibSL::Math::V3F(0, 0, 1), 0);
m_Translation = 0;
m_Center = 0;
m_Width = 0;
m_Height = 0;
m_PrevX = 0;
m_PrevY = 0;
m_Elapsed = 0;
m_Status = 0;
m_Radius = 1.0f;
m_Walkthrough = false;
m_WalkDir = 0.0f;
m_WalkSide = 0.0f;
m_Up = Z_pos;
m_WalkSpeed = 1.0f;
m_BallSpeed = 1.0f;
m_Locked = false;
}
CMUK_ERROR_CODE cmuk::computeLegTransforms( LegIndex leg,
const vec3f& q,
Transform3f t[4] ) const {
if ((int)leg < 0 || (int)leg >= NUM_LEGS) {
return CMUK_BAD_LEG_INDEX;
}
if (!t) {
return CMUK_INSUFFICIENT_ARGUMENTS;
}
t[0] = Transform3f::rx(q[0], jo(_kc, leg, HIP_RX_OFFSET, _centeredFootIK));
t[1] = t[0] * Transform3f::ry(q[1], jo(_kc, leg, HIP_RY_OFFSET, _centeredFootIK));
t[2] = t[1] * Transform3f::ry(q[2], jo(_kc, leg, KNEE_RY_OFFSET, _centeredFootIK));
t[3] = t[2] * Transform3f(quatf(0.0f, 0.0f, 0.0f, 1.0f),
jo(_kc, leg, FOOT_OFFSET, _centeredFootIK));
return CMUK_OKAY;
}
void cmuk::cacheTransforms(const KState& q, XformCache* c) const {
c->absXforms[0] = c->relXforms[0] = Transform3f(q.body_rot, q.body_pos);
const bool& cfik = _centeredFootIK;
size_t fidx = 0;
for (int leg=0; leg<NUM_LEGS; ++leg) {
++fidx;
c->relXforms[fidx] = Transform3f::rx(q.leg_rot[leg][0],
jo(_kc, leg, HIP_RX_OFFSET, cfik));
c->absXforms[fidx] = c->absXforms[FRAME_TRUNK] * c->relXforms[fidx];
++fidx;
c->relXforms[fidx] = Transform3f::ry(q.leg_rot[leg][1],
jo(_kc, leg, HIP_RY_OFFSET, cfik));
c->absXforms[fidx] = c->absXforms[fidx-1] * c->relXforms[fidx];
++fidx;
c->relXforms[fidx] = Transform3f::ry(q.leg_rot[leg][2],
jo(_kc, leg, KNEE_RY_OFFSET, cfik));
c->absXforms[fidx] = c->absXforms[fidx-1] * c->relXforms[fidx];
++fidx;
c->relXforms[fidx] = Transform3f(quatf(0.0f, 0.0f, 0.0f, 1.0f),
jo(_kc, leg, FOOT_OFFSET, cfik));
c->absXforms[fidx] = c->absXforms[fidx-1] * c->relXforms[fidx];
}
}
void initRigidBody(MObject &node)
{
MFnDagNode fnDagNode(node);
rigidBodyNode *rbNode = static_cast<rigidBodyNode*>(fnDagNode.userNode());
rigid_body_t::pointer rb = rbNode->rigid_body();
if(fnDagNode.parentCount() == 0) {
std::cout << "No transform found!" << std::endl;
return;
}
MFnTransform fnTransform(fnDagNode.parent(0));
MPlug plgMass(node, rigidBodyNode::ia_mass);
float mass = 0.f;
plgMass.getValue(mass);
if(mass>0.f) {
//active rigid body, set the world transform from the initial* attributes
MObject obj;
vec3f pos;
MPlug plgInitialValue(node, rigidBodyNode::ia_initialPosition);
plgInitialValue.child(0).getValue(pos[0]);
plgInitialValue.child(1).getValue(pos[1]);
plgInitialValue.child(2).getValue(pos[2]);
vec3f rot;
plgInitialValue.setAttribute(rigidBodyNode::ia_initialRotation);
plgInitialValue.child(0).getValue(rot[0]);
plgInitialValue.child(1).getValue(rot[1]);
plgInitialValue.child(2).getValue(rot[2]);
vec3f vel;
plgInitialValue.setAttribute(rigidBodyNode::ia_initialVelocity);
plgInitialValue.child(0).getValue(vel[0]);
plgInitialValue.child(1).getValue(vel[1]);
plgInitialValue.child(2).getValue(vel[2]);
vec3f spin;
plgInitialValue.setAttribute(rigidBodyNode::ia_initialSpin);
plgInitialValue.child(0).getValue(spin[0]);
plgInitialValue.child(1).getValue(spin[1]);
plgInitialValue.child(2).getValue(spin[2]);
MEulerRotation meuler(deg2rad(rot[0]), deg2rad(rot[1]), deg2rad(rot[2]));
MQuaternion mquat = meuler.asQuaternion();
rb->set_transform(pos, quatf(mquat.w, mquat.x, mquat.y, mquat.z));
rb->set_linear_velocity(vel);
rb->set_angular_velocity(spin);
rb->set_kinematic(false);
fnTransform.setRotation(meuler);
fnTransform.setTranslation(MVector(pos[0], pos[1], pos[2]), MSpace::kTransform);
} else {
//passive rigid body, get the world trasform from from the shape node
MQuaternion mquat;
fnTransform.getRotation(mquat);
MVector mpos(fnTransform.getTranslation(MSpace::kTransform));
rb->set_transform(vec3f(mpos.x, mpos.y, mpos.z), quatf(mquat.w, mquat.x, mquat.y, mquat.z));
rb->set_kinematic(true);
}
}
bool GameLogic::Initialize( instances_t* instances ) {
Instance = *instances;
Instance.InputHandler->m_Mouse.RelativeMouse = true;
Instance.InputHandler->UpdateAction(
HashStringCRC32( "GAME_CHANGE_MODE" ),
{ 1, std::bind( []() {
if( Instance.GameLogic->GetActiveGameMode() == GAME_LOGIC_MODE_EDITOR ) {
Instance.InputHandler->m_Mouse.SetRelative( true );
Instance.InputHandler->UpdateRelativeMouse();
Instance.SceneManager->SetActiveCamera( Instance.SceneManager->GetCameraFirstPerson() );
Instance.GameLogic->SetActiveGameMode( GAME_LOGIC_MODE_PLAYING );
} else {
Instance.InputHandler->m_Mouse.SetRelative( false );
Instance.SceneManager->SetActiveCamera( Instance.SceneManager->GetCameraEditor() );
Instance.GameLogic->SetActiveGameMode( GAME_LOGIC_MODE_EDITOR );
}
} ) } );
m_MouseCursor = RENDER_ALLOCATE( renderable_t );
m_MouseCursor->Type = RENDERABLE_TYPE_UI_TEXTURE;
renderable_ui_texture_t* cursorObj = RENDER_ALLOCATE( renderable_ui_texture_t );
cursorObj->Width = 32;
cursorObj->Height = 32;
cursorObj->Texture = Instance.TextureManager->GetTexture2D( "Interface/cursor" );
m_MouseCursor->Object = cursorObj;
m_MouseDecal = RENDER_ALLOCATE( renderable_t );
m_MouseDecal->Type = RENDERABLE_TYPE_DECAL;
decal_t* mouseDecal = RENDER_ALLOCATE( decal_t );
m_MouseDecal->Object = mouseDecal;
mouseDecal->WorldPosition = vec3f( 0.0f, 1.0f, 0.0f );
mouseDecal->Scale = vec3f( 32.0f, 32.0f, 64.0f );
mouseDecal->Rotation = quatf();
mouseDecal->ModelMatrix.Identity();
mouseDecal->ModelMatrix.Translate( mouseDecal->WorldPosition );
mouseDecal->ModelMatrix.Rotate( mouseDecal->Rotation );
mouseDecal->ModelMatrix.Scale( mouseDecal->Scale );
mouseDecal->Material = RENDER_ALLOCATE( material_t );
mouseDecal->Material->Data = RENDER_ALLOCATE( material_data_t );
mouseDecal->Material->Hashcode = HashStringCRC32( "Mouse_Decal" );
mouseDecal->Material->Index = Instance.Renderer->AllocateMaterialSlot( mouseDecal->Material->Data );
mouseDecal->Material->Layers[TEXTURE_LAYER_DIFFUSE] = Instance.TextureManager->GetTexture2D( "Editor/C_terrain_decal" );
mouseDecal->Material->Layers[TEXTURE_LAYER_NORMAL] = Instance.TextureManager->GetTexture2D( "Editor/C_terrain_decal_normal" );
mouseDecal->Material->Data->Alpha = 255;
mouseDecal->Material->Data->DiffuseColor = colorrgbf( 1.0f );
mouseDecal->Material->Data->DSSHEFactors = vec4f( 0.800000012f, 0.498039216f, 0.0941176489f, 0.000000000f );
mouseDecal->Material->Data->Flags = MATERIAL_FLAG_IS_DECAL;
mouseDecal->Material->Data->Shading = 17;
mouseDecal->Material->Data->SpecularColor = colorrgbf( 1.0f );
mouseDecal->Material->Build();
#ifdef FLAG_DEBUG
Instance.SceneManager->Create( true );
#else
Instance.SceneManager->Create( false );
#endif
return true;
}
void sixdofConstraintNode::computeWorldMatrix(const MPlug& plug, MDataBlock& data)
{
MObject thisObject(thisMObject());
MFnDagNode fnDagNode(thisObject);
MObject update;
MPlug(thisObject, ca_constraint).getValue(update);
MPlug(thisObject, ca_constraintParam).getValue(update);
MStatus status;
MFnTransform fnParentTransform(fnDagNode.parent(0, &status));
double fixScale[3] = { 1., 1., 1. }; // lock scale
fnParentTransform.setScale(fixScale);
MVector mtranslation = fnParentTransform.getTranslation(MSpace::kTransform, &status);
if(bSolverNode::isStartTime)
{ // allow to edit pivots
MPlug plgRigidBodyA(thisObject, ia_rigidBodyA);
MPlug plgRigidBodyB(thisObject, ia_rigidBodyB);
MObject update;
//force evaluation of the rigidBody
plgRigidBodyA.getValue(update);
if(plgRigidBodyA.isConnected())
{
MPlugArray connections;
plgRigidBodyA.connectedTo(connections, true, true);
if(connections.length() != 0)
{
MFnDependencyNode fnNode(connections[0].node());
if(fnNode.typeId() == boingRBNode::typeId)
{
MObject rbAObj = fnNode.object();
boingRBNode *pRigidBodyNodeA = static_cast<boingRBNode*>(fnNode.userNode());
MPlug(rbAObj, pRigidBodyNodeA->worldMatrix).elementByLogicalIndex(0).getValue(update);
}
}
}
plgRigidBodyB.getValue(update);
if(plgRigidBodyB.isConnected())
{
MPlugArray connections;
plgRigidBodyB.connectedTo(connections, true, true);
if(connections.length() != 0)
{
MFnDependencyNode fnNode(connections[0].node());
if(fnNode.typeId() == boingRBNode::typeId)
{
MObject rbBObj = fnNode.object();
boingRBNode *pRigidBodyNodeB = static_cast<boingRBNode*>(fnNode.userNode());
MPlug(rbBObj, pRigidBodyNodeB->worldMatrix).elementByLogicalIndex(0).getValue(update);
}
}
}
if(m_constraint)
{
MQuaternion mrotation;
fnParentTransform.getRotation(mrotation, MSpace::kTransform);
bool doUpdatePivot = m_constraint->getPivotChanged();
if(!doUpdatePivot)
{
vec3f worldP;
quatf worldR;
m_constraint->get_world(worldP, worldR);
float deltaPX = worldP[0] - float(mtranslation.x);
float deltaPY = worldP[1] - float(mtranslation.y);
float deltaPZ = worldP[2] - float(mtranslation.z);
float deltaRX = (float)mrotation.x - worldR[1];
float deltaRY = (float)mrotation.y - worldR[2];
float deltaRZ = (float)mrotation.z - worldR[3];
float deltaRW = (float)mrotation.w - worldR[0];
float deltaSq = deltaPX * deltaPX + deltaPY * deltaPY + deltaPZ * deltaPZ
+ deltaRX * deltaRX + deltaRY * deltaRY + deltaRZ * deltaRZ + deltaRW * deltaRW;
doUpdatePivot = (deltaSq > FLT_EPSILON);
}
if(doUpdatePivot)
{
m_constraint->set_world(vec3f((float) mtranslation[0], (float) mtranslation[1], (float) mtranslation[2]),
quatf((float)mrotation.w, (float)mrotation.x, (float)mrotation.y, (float)mrotation.z));
vec3f pivInA, pivInB;
quatf rotInA, rotInB;
m_constraint->get_frameA(pivInA, rotInA);
m_constraint->get_frameB(pivInB, rotInB);
MDataHandle hPivInA = data.outputValue(ia_pivotInA);
float3 &ihPivInA = hPivInA.asFloat3();
MDataHandle hPivInB = data.outputValue(ia_pivotInB);
float3 &ihPivInB = hPivInB.asFloat3();
for(int i = 0; i < 3; i++)
{
ihPivInA[i] = pivInA[i];
ihPivInB[i] = pivInB[i];
}
MDataHandle hRotInA = data.outputValue(ia_rotationInA);
float3 &hrotInA = hRotInA.asFloat3();
MQuaternion mrotA(rotInA[1], rotInA[2], rotInA[3], rotInA[0]);
MEulerRotation newrotA(mrotA.asEulerRotation());
hrotInA[0] = rad2deg((float)newrotA.x);
hrotInA[1] = rad2deg((float)newrotA.y);
hrotInA[2] = rad2deg((float)newrotA.z);
MDataHandle hRotInB = data.outputValue(ia_rotationInB);
float3 &hrotInB = hRotInB.asFloat3();
MQuaternion mrotB(rotInB[1], rotInB[2], rotInB[3], rotInB[0]);
MEulerRotation newrotB(mrotB.asEulerRotation());
hrotInB[0] = rad2deg((float)newrotB.x);
hrotInB[1] = rad2deg((float)newrotB.y);
hrotInB[2] = rad2deg((float)newrotB.z);
m_constraint->setPivotChanged(false);
m_constraint->get_local_frameA(m_PivInA, m_RotInA);
m_constraint->get_local_frameB(m_PivInB, m_RotInB);
}
}
}
else
{ // if not start time, lock position and rotation
if(m_constraint)
{
vec3f worldP;
quatf worldR;
m_constraint->get_world(worldP, worldR);
fnParentTransform.setTranslation(MVector(worldP[0], worldP[1], worldP[2]), MSpace::kTransform);
fnParentTransform.setRotation(MQuaternion(worldR[1], worldR[2], worldR[3], worldR[0]));
}
}
m_initialized = true;
data.setClean(plug);
}
//gather previous and current frame transformations for substep interpolation
void dSolverNode::gatherPassiveTransforms(MPlugArray &rbConnections, std::vector<xforms_t> &xforms)
{
xforms.resize(0);
xforms_t xform;
for(size_t i = 0; i < rbConnections.length(); ++i) {
MObject node = rbConnections[i].node();
MFnDagNode fnDagNode(node);
if(fnDagNode.typeId() == rigidBodyNode::typeId) {
rigidBodyNode *rbNode = static_cast<rigidBodyNode*>(fnDagNode.userNode());
rigid_body_t::pointer rb = rbNode->rigid_body();
if(fnDagNode.parentCount() == 0) {
std::cout << "No transform found!" << std::endl;
continue;
}
MFnTransform fnTransform(fnDagNode.parent(0));
MPlug plgMass(node, rigidBodyNode::ia_mass);
float mass = 0.f;
plgMass.getValue(mass);
bool active = (mass>0.f);
if(!active) {
MQuaternion mquat;
fnTransform.getRotation(mquat);
MVector mpos(fnTransform.getTranslation(MSpace::kTransform));
rb->get_transform(xform.m_x0, xform.m_q0);
xform.m_x1 = vec3f(mpos.x, mpos.y, mpos.z);
xform.m_q1 = quatf(mquat.w, mquat.x, mquat.y, mquat.z);
xforms.push_back(xform);
}
} else if(fnDagNode.typeId() == rigidBodyArrayNode::typeId) {
rigidBodyArrayNode *rbNode = static_cast<rigidBodyArrayNode*>(fnDagNode.userNode());
std::vector<rigid_body_t::pointer>& rbs = rbNode->rigid_bodies();
if(fnDagNode.parentCount() == 0) {
std::cout << "No transform found!" << std::endl;
return;
}
MPlug plgMass(node, rigidBodyArrayNode::ia_mass);
float mass = 0.f;
plgMass.getValue(mass);
bool active = (mass>0.f);
if(!active) {
MPlug plgPosition(node, rigidBodyArrayNode::io_position);
MPlug plgRotation(node, rigidBodyArrayNode::io_rotation);
MPlug plgElement;
for(size_t j = 0; j < rbs.size(); ++j) {
rbs[j]->get_transform(xform.m_x0, xform.m_q0);
plgElement = plgPosition.elementByLogicalIndex(j);
plgElement.child(0).getValue(xform.m_x1[0]);
plgElement.child(1).getValue(xform.m_x1[1]);
plgElement.child(2).getValue(xform.m_x1[2]);
vec3f rot;
plgElement = plgRotation.elementByLogicalIndex(j);
plgElement.child(0).getValue(rot[0]);
plgElement.child(1).getValue(rot[1]);
plgElement.child(2).getValue(rot[2]);
MEulerRotation meuler(deg2rad(rot[0]), deg2rad(rot[1]), deg2rad(rot[2]));
MQuaternion mquat = meuler.asQuaternion();
xform.m_q1 = quatf(mquat.w, mquat.x, mquat.y, mquat.z);
xforms.push_back(xform);
}
}
}
}
}
void NAMESPACE::Trackball::reset()
{
m_Translation = V3F(0, 0, -m_Radius);
m_Rotation = quatf(V3F(0, 0, 1), 0.0f);
}
void initRigidBodyArray(MObject &node)
{
MFnDagNode fnDagNode(node);
rigidBodyArrayNode *rbNode = static_cast<rigidBodyArrayNode*>(fnDagNode.userNode());
std::vector<rigid_body_t::pointer>& rbs = rbNode->rigid_bodies();
if(fnDagNode.parentCount() == 0) {
std::cout << "No transform found!" << std::endl;
return;
}
MFnTransform fnTransform(fnDagNode.parent(0));
MPlug plgMass(node, rigidBodyArrayNode::ia_mass);
float mass = 0.f;
plgMass.getValue(mass);
bool active = (mass>0.f);
if(active) {
//active rigid body, set the world transform from the initial* attributes
MObject obj;
MPlug plgInitialPosition(node, rigidBodyArrayNode::ia_initialPosition);
MPlug plgInitialRotation(node, rigidBodyArrayNode::ia_initialRotation);
MPlug plgInitialVelocity(node, rigidBodyArrayNode::ia_initialVelocity);
MPlug plgInitialSpin(node, rigidBodyArrayNode::ia_initialSpin);
MPlug plgPosition(node, rigidBodyArrayNode::io_position);
MPlug plgRotation(node, rigidBodyArrayNode::io_rotation);
MPlug plgElement;
for(size_t j = 0; j < rbs.size(); ++j) {
vec3f pos;
plgElement = plgInitialPosition.elementByLogicalIndex(j);
plgElement.child(0).getValue(pos[0]);
plgElement.child(1).getValue(pos[1]);
plgElement.child(2).getValue(pos[2]);
vec3f rot;
plgElement = plgInitialRotation.elementByLogicalIndex(j);
plgElement.child(0).getValue(rot[0]);
plgElement.child(1).getValue(rot[1]);
plgElement.child(2).getValue(rot[2]);
vec3f vel;
plgElement = plgInitialVelocity.elementByLogicalIndex(j);
plgElement.child(0).getValue(vel[0]);
plgElement.child(1).getValue(vel[1]);
plgElement.child(2).getValue(vel[2]);
vec3f spin;
plgElement = plgInitialSpin.elementByLogicalIndex(j);
plgElement.child(0).getValue(spin[0]);
plgElement.child(1).getValue(spin[1]);
plgElement.child(2).getValue(spin[2]);
MEulerRotation meuler(deg2rad(rot[0]), deg2rad(rot[1]), deg2rad(rot[2]));
MQuaternion mquat = meuler.asQuaternion();
rbs[j]->set_transform(pos, quatf(mquat.w, mquat.x, mquat.y, mquat.z));
rbs[j]->set_linear_velocity(vel);
rbs[j]->set_angular_velocity(spin);
rbs[j]->set_kinematic(false);
plgElement = plgPosition.elementByLogicalIndex(j);
plgElement.child(0).setValue(pos[0]);
plgElement.child(1).setValue(pos[1]);
plgElement.child(2).setValue(pos[2]);
plgElement = plgRotation.elementByLogicalIndex(j);
plgElement.child(0).setValue(rot[0]);
plgElement.child(1).setValue(rot[1]);
plgElement.child(2).setValue(rot[2]);
}
} else {
//passive rigid body, get the world trasform from from the position/rotation attributes
MPlug plgPosition(node, rigidBodyArrayNode::io_position);
MPlug plgRotation(node, rigidBodyArrayNode::io_rotation);
MPlug plgElement;
for(size_t j = 0; j < rbs.size(); ++j) {
vec3f pos;
plgElement = plgPosition.elementByLogicalIndex(j);
plgElement.child(0).getValue(pos[0]);
plgElement.child(1).getValue(pos[1]);
plgElement.child(2).getValue(pos[2]);
vec3f rot;
plgElement = plgRotation.elementByLogicalIndex(j);
plgElement.child(0).getValue(rot[0]);
plgElement.child(1).getValue(rot[1]);
plgElement.child(2).getValue(rot[2]);
MEulerRotation meuler(deg2rad(rot[0]), deg2rad(rot[1]), deg2rad(rot[2]));
MQuaternion mquat = meuler.asQuaternion();
rbs[j]->set_transform(pos, quatf(mquat.w, mquat.x, mquat.y, mquat.z));
rbs[j]->set_kinematic(false);
}
}
}
void boing::createRigidBody()
{
//MGlobal::getActiveSelectionList(m_undoSelectionList);
if (!name.length()) {
name = "procBoingRb1";
}
std::cout<<"name in boing::createRigidBody() "<<name<<endl;
double mscale[3] = {1,1,1};
MQuaternion mrotation = MEulerRotation(m_initial_rotation).asQuaternion();
//collision_shape_t::pointer collision_shape;
if(!m_collision_shape) {
//not connected to a collision shape, put a default one
m_collision_shape = solver_t::create_box_shape();
} else {
if ( !node.isNull() ) {
MFnDagNode fnDagNode(node);
//cout<<"node : "<<fnDagNode.partialPathName()<<endl;
MFnTransform fnTransform(fnDagNode.parent(0));
//cout<<"MFnTransform node : "<<fnTransform.partialPathName()<<endl;
if ( m_initial_position == MVector::zero ) {
m_initial_position = fnTransform.getTranslation(MSpace::kTransform);
}
if ( m_initial_rotation == MVector::zero ) {
fnTransform.getRotation(mrotation, MSpace::kTransform);
}
fnTransform.getScale(mscale);
}
}
shared_ptr<solver_impl_t> solv = solver_t::get_solver();
m_rigid_body = solver_t::create_rigid_body(m_collision_shape);
m_rigid_body->set_transform(vec3f((float)m_initial_position.x, (float)m_initial_position.y, (float)m_initial_position.z),
quatf((float)mrotation.w, (float)mrotation.x, (float)mrotation.y, (float)mrotation.z));
m_rigid_body->set_linear_velocity( vec3f((float)m_initial_velocity.x,(float)m_initial_velocity.y,(float)m_initial_velocity.z) );
m_rigid_body->set_angular_velocity( vec3f((float)m_initial_angularvelocity.x,(float)m_initial_angularvelocity.y,(float)m_initial_angularvelocity.z) );
m_rigid_body->collision_shape()->set_scale(vec3f((float)mscale[0], (float)mscale[1], (float)mscale[2]));
/*
float mass = 1.f;
m_rigid_body->set_mass(mass);
m_rigid_body->set_inertia((float)mass * m_rigid_body->collision_shape()->local_inertia());
*/
//float mass = 0.f;
if (typeName == boingRBNode::typeName) {
MPlug(node, boingRBNode::ia_mass).getValue(m_mass);
}
float curMass = m_rigid_body->get_mass();
bool changedMassStatus= false;
if ((curMass > 0.f) != (m_mass > 0.f))
{
changedMassStatus = true;
}
if (changedMassStatus)
solver_t::remove_rigid_body(m_rigid_body);
m_rigid_body->set_mass(m_mass);
m_rigid_body->set_inertia((float)m_mass * m_rigid_body->collision_shape()->local_inertia());
if (changedMassStatus)
solver_t::add_rigid_body(m_rigid_body, name.asChar());
//initialize those default values too
float restitution = 0.3f;
//MPlug(thisObject, rigidBodyNode::ia_restitution).getValue(restitution);
m_rigid_body->set_restitution(restitution);
float friction = 0.5f;
//MPlug(thisObject, rigidBodyNode::ia_friction).getValue(friction);
m_rigid_body->set_friction(friction);
float linDamp = 0.f;
//MPlug(thisObject, rigidBodyNode::ia_linearDamping).getValue(linDamp);
m_rigid_body->set_linear_damping(linDamp);
float angDamp = 0.2f;
//MPlug(thisObject, rigidBodyNode::ia_angularDamping).getValue(angDamp);
m_rigid_body->set_angular_damping(angDamp);
m_rigid_body->impl()->body()->setUserPointer((void*) this);
}
/*
Rigid bodies are added to the solver here
*/
void rigidBodyNode::computeRigidBody(const MPlug& plug, MDataBlock& data)
{
MObject thisObject(thisMObject());
MPlug plgCollisionShape(thisObject, ia_collisionShape);
MObject update;
//force evaluation of the shape
plgCollisionShape.getValue(update);
// The following two variables are not actually used!
vec3f prevCenter(0, 0, 0);
quatf prevRotation(qidentity<float>());
if(m_rigid_body) {
prevCenter = m_rigid_body->collision_shape()->center();
prevRotation = m_rigid_body->collision_shape()->rotation();
}
collision_shape_t::pointer collision_shape;
if(plgCollisionShape.isConnected()) {
MPlugArray connections;
plgCollisionShape.connectedTo(connections, true, true);
if(connections.length() != 0) {
MFnDependencyNode fnNode(connections[0].node());
if(fnNode.typeId() == collisionShapeNode::typeId) {
collisionShapeNode *pCollisionShapeNode = static_cast<collisionShapeNode*>(fnNode.userNode());
collision_shape = pCollisionShapeNode->collisionShape();
} else {
std::cout << "rigidBodyNode connected to a non-collision shape node!" << std::endl;
}
}
}
if(!collision_shape) {
//not connected to a collision shape, put a default one
collision_shape = solver_t::create_sphere_shape();
}
solver_t::remove_rigid_body(m_rigid_body);
m_rigid_body = solver_t::create_rigid_body(collision_shape);
solver_t::add_rigid_body(m_rigid_body,name().asChar());
// once at creation/load time : get transform from Maya transform node
MFnDagNode fnDagNode(thisObject);
MFnTransform fnTransform(fnDagNode.parent(0));
MVector mtranslation = fnTransform.getTranslation(MSpace::kTransform);
MQuaternion mrotation;
fnTransform.getRotation(mrotation, MSpace::kTransform);
double mscale[3];
fnTransform.getScale(mscale);
m_rigid_body->set_transform(vec3f((float)mtranslation.x, (float)mtranslation.y, (float)mtranslation.z),
quatf((float)mrotation.w, (float)mrotation.x, (float)mrotation.y, (float)mrotation.z));
m_rigid_body->collision_shape()->set_scale(vec3f((float)mscale[0], (float)mscale[1], (float)mscale[2]));
float mass = 0.f;
MPlug(thisObject, rigidBodyNode::ia_mass).getValue(mass);
float curMass = m_rigid_body->get_mass();
bool changedMassStatus= false;
if ((curMass > 0.f) != (mass > 0.f))
{
changedMassStatus = true;
}
if (changedMassStatus)
solver_t::remove_rigid_body(m_rigid_body);
m_rigid_body->set_mass(mass);
m_rigid_body->set_inertia((float)mass * m_rigid_body->collision_shape()->local_inertia());
if (changedMassStatus)
solver_t::add_rigid_body(m_rigid_body, name().asChar());
//initialize those default values too
float restitution = 0.f;
MPlug(thisObject, rigidBodyNode::ia_restitution).getValue(restitution);
m_rigid_body->set_restitution(restitution);
float friction = 0.5f;
MPlug(thisObject, rigidBodyNode::ia_friction).getValue(friction);
m_rigid_body->set_friction(friction);
float linDamp = 0.f;
MPlug(thisObject, rigidBodyNode::ia_linearDamping).getValue(linDamp);
m_rigid_body->set_linear_damping(linDamp);
float angDamp = 0.f;
MPlug(thisObject, rigidBodyNode::ia_angularDamping).getValue(angDamp);
m_rigid_body->set_angular_damping(angDamp);
/*
//this is not necessary, initialize linear/angular velocity (spin) is already set at initRigidBodyArray in dSolverNode.cpp
MPlug ilv(thisObject, rigidBodyNode::ia_initialVelocity);
MDataHandle hInitLinVel= ilv.asMDataHandle();
float3 &initLinVel= hInitLinVel.asFloat3();
vec3f lv(initLinVel[0],initLinVel[1],initLinVel[2]);
m_rigid_body->set_linear_velocity(lv);
*/
//?? the next line crashes Maya 2014. Note sure what it is supposed to achieve.
// data.outputValue(ca_rigidBody).set(true);
data.setClean(plug);
}
MStatus boingRbCmd::createRigidBody(collision_shape_t::pointer collision_shape,
MObject node,
MString name,
MVector vel,
MVector pos,
MVector rot)
{
//MGlobal::getActiveSelectionList(m_undoSelectionList);
if (!name.length()) {
name = "dRigidBody";
}
double mscale[3] = {1,1,1};
MQuaternion mrotation = MEulerRotation(rot).asQuaternion();
//collision_shape_t::pointer collision_shape;
if(!collision_shape) {
//not connected to a collision shape, put a default one
collision_shape = solver_t::create_sphere_shape();
} else {
if ( rot == MVector::zero || pos == MVector::zero ) {
MFnDagNode fnDagNode(node);
//cout<<"node : "<<fnDagNode.partialPathName()<<endl;
MFnTransform fnTransform(fnDagNode.parent(0));
//cout<<"MFnTransform node : "<<fnTransform.partialPathName()<<endl;
pos = fnTransform.getTranslation(MSpace::kTransform);
fnTransform.getRotation(mrotation, MSpace::kTransform);
fnTransform.getScale(mscale);
}
}
//cout<<"removing m_rigid_body"<<endl;
//solver_t::remove_rigid_body(m_rigid_body);
cout<<"register name : "<<name<<endl;
shared_ptr<solver_impl_t> solv = solver_t::get_solver();
rigid_body_t::pointer m_rigid_body = solver_t::create_rigid_body(collision_shape);
solver_t::add_rigid_body(m_rigid_body, name.asChar());
//cout<<"transform : "<<pos<<endl;
//cout<<"rotation : "<<rot<<endl;
//cout<<"velocity : "<<vel<<endl;
//m_rigid_body->collision_shape()->set_user_pointer(name);
//const rigid_body_impl_t* rb = static_cast<const rigid_body_impl_t*>(m_rigid_body.get());
const rigid_body_impl_t* rb = m_rigid_body->impl();
//rigidBodyNode *rbNode = static_cast<rigidBodyNode*>(rb->get());
//rb->register_name(solv.get(),rbNode->name().asChar());
solv->register_name(rb, name.asChar());
m_rigid_body->set_transform(vec3f((float)pos.x, (float)pos.y, (float)pos.z),
quatf((float)mrotation.w, (float)mrotation.x, (float)mrotation.y, (float)mrotation.z));
m_rigid_body->collision_shape()->set_scale(vec3f((float)mscale[0], (float)mscale[1], (float)mscale[2]));
float mass = 1.f;
//MPlug(thisObject, rigidBodyNode::ia_mass).getValue(mass);
/*
float curMass = m_rigid_body->get_mass();
bool changedMassStatus= false;
if ((curMass > 0.f) != (mass > 0.f))
{
changedMassStatus = true;
}
if (changedMassStatus) solver_t::remove_rigid_body(m_rigid_body);
*/
m_rigid_body->set_mass(mass);
m_rigid_body->set_inertia((float)mass * m_rigid_body->collision_shape()->local_inertia());
//if (changedMassStatus)
// solver_t::add_rigid_body(m_rigid_body, name.asChar());
//initialize those default values too
float restitution = 0.3f;
//MPlug(thisObject, rigidBodyNode::ia_restitution).getValue(restitution);
m_rigid_body->set_restitution(restitution);
float friction = 0.5f;
//MPlug(thisObject, rigidBodyNode::ia_friction).getValue(friction);
m_rigid_body->set_friction(friction);
float linDamp = 0.f;
//MPlug(thisObject, rigidBodyNode::ia_linearDamping).getValue(linDamp);
m_rigid_body->set_linear_damping(linDamp);
float angDamp = 0.2f;
//MPlug(thisObject, rigidBodyNode::ia_angularDamping).getValue(angDamp);
m_rigid_body->set_angular_damping(angDamp);
//shared_ptr<solver_impl_t> solv = solver_t::get_solver();
//const char *namePtr = solv->m_nameMap.find(m_rigid_body);
//const char* rbname = MySerializer::findNameForPointer(m_rigid_body);
//cout<<"rbname = "<<namePtr<<endl;
//solv.get()->dynamicsWorld();
return MS::kSuccess;
}
void rigidBodyNode::computeWorldMatrix(const MPlug& plug, MDataBlock& data)
{
if (!m_rigid_body)
return;
// std::cout << "rigidBodyNode::computeWorldMatrix" << std::endl;
MObject thisObject(thisMObject());
MFnDagNode fnDagNode(thisObject);
MObject update;
MPlug(thisObject, ca_rigidBody).getValue(update);
MPlug(thisObject, ca_rigidBodyParam).getValue(update);
vec3f pos;
quatf rot;
MStatus status;
MFnTransform fnParentTransform(fnDagNode.parent(0, &status));
double mscale[3];
fnParentTransform.getScale(mscale);
m_rigid_body->get_transform(pos, rot);
if(dSolverNode::isStartTime)
{ // allow to edit ptranslation and rotation
MVector mtranslation = fnParentTransform.getTranslation(MSpace::kTransform, &status);
MQuaternion mrotation;
fnParentTransform.getRotation(mrotation, MSpace::kTransform);
float deltaPX = (float)mtranslation.x - pos[0];
float deltaPY = (float)mtranslation.y - pos[1];
float deltaPZ = (float)mtranslation.z - pos[2];
float deltaRX = (float)mrotation.x - rot[1];
float deltaRY = (float)mrotation.y - rot[2];
float deltaRZ = (float)mrotation.z - rot[3];
float deltaRW = (float)mrotation.w - rot[0];
float deltaSq = deltaPX * deltaPX + deltaPY * deltaPY + deltaPZ * deltaPZ
+ deltaRX * deltaRX + deltaRY * deltaRY + deltaRZ * deltaRZ + deltaRW * deltaRW;
if(deltaSq > FLT_EPSILON)
{
m_rigid_body->set_transform(vec3f((float)mtranslation.x, (float)mtranslation.y, (float)mtranslation.z),
quatf((float)mrotation.w, (float)mrotation.x, (float)mrotation.y, (float)mrotation.z));
m_rigid_body->set_interpolation_transform(vec3f((float)mtranslation.x, (float)mtranslation.y, (float)mtranslation.z),
quatf((float)mrotation.w, (float)mrotation.x, (float)mrotation.y, (float)mrotation.z));
m_rigid_body->update_constraint();
MDataHandle hInitPos = data.outputValue(ia_initialPosition);
float3 &ihpos = hInitPos.asFloat3();
ihpos[0] = (float)mtranslation.x;
ihpos[1] = (float)mtranslation.y;
ihpos[2] = (float)mtranslation.z;
MDataHandle hInitRot = data.outputValue(ia_initialRotation);
float3 &ihrot = hInitRot.asFloat3();
MEulerRotation newrot(mrotation.asEulerRotation());
ihrot[0] = rad2deg((float)newrot.x);
ihrot[1] = rad2deg((float)newrot.y);
ihrot[2] = rad2deg((float)newrot.z);
}
}
else
{ // if not start time, lock position and rotation for active rigid bodies
float mass = 0.f;
MPlug(thisObject, rigidBodyNode::ia_mass).getValue(mass);
if(mass > 0.f)
{
fnParentTransform.setTranslation(MVector(pos[0], pos[1], pos[2]), MSpace::kTransform);
fnParentTransform.setRotation(MQuaternion(rot[1], rot[2], rot[3], rot[0]));
}
}
float mass = 0.f;
MPlug(thisObject, rigidBodyNode::ia_mass).getValue(mass);
float curMass = m_rigid_body->get_mass();
bool changedMassStatus= false;
if ((curMass > 0.f) != (mass > 0.f))
{
changedMassStatus = true;
}
if (changedMassStatus)
solver_t::remove_rigid_body(m_rigid_body);
m_rigid_body->set_mass(mass);
m_rigid_body->set_inertia((float)mass * m_rigid_body->collision_shape()->local_inertia());
if (changedMassStatus)
solver_t::remove_rigid_body(m_rigid_body);
float restitution = 0.f;
MPlug(thisObject, rigidBodyNode::ia_restitution).getValue(restitution);
m_rigid_body->set_restitution(restitution);
float friction = 0.5f;
MPlug(thisObject, rigidBodyNode::ia_friction).getValue(friction);
m_rigid_body->set_friction(friction);
float linDamp = 0.f;
MPlug(thisObject, rigidBodyNode::ia_linearDamping).getValue(linDamp);
m_rigid_body->set_linear_damping(linDamp);
float angDamp = 0.f;
MPlug(thisObject, rigidBodyNode::ia_angularDamping).getValue(angDamp);
m_rigid_body->set_angular_damping(angDamp);
data.setClean(plug);
//set the scale to the collision shape
m_rigid_body->collision_shape()->set_scale(vec3f((float)mscale[0], (float)mscale[1], (float)mscale[2]));
}
