void dNewtonBody::SetPosition(dFloat x, dFloat y, dFloat z) { dQuaternion rot; NewtonBodyGetRotation(m_body, &rot.m_q0); dMatrix mat(rot, dVector(x, y, z)); NewtonBodySetMatrix(m_body, &mat[0][0]); }
dVector CustomPlayerController::CalculateDesiredOmega (dFloat headingAngle, dFloat timestep) const { dQuaternion playerRotation; dQuaternion targetRotation (m_upVector, headingAngle); NewtonBodyGetRotation(m_body, &playerRotation.m_q0); return playerRotation.CalcAverageOmega (targetRotation, 0.5f / timestep); }
// Transform callback to set the matrix of a the visual entity void SetTransformCallback (const NewtonBody* body, const float* matrix, int threadIndex) { NewtonEntity* ent; // Get the position from the matrix glm::vec4 position (matrix[12], matrix[13], matrix[14], 1.0f); glm::quat rotation; // we will ignore the Rotation part of matrix and use the quaternion rotation stored in the body NewtonBodyGetRotation(body, &rotation[0]); // get the entity associated with this rigid body ent = (NewtonEntity*) NewtonBodyGetUserData(body); // since this tutorial run the physics and a different fps than the Graphics // we need to save the entity current transformation state before updating the new state. ent->prevPosition = ent->curPosition; ent->prevRotation = ent->curRotation; if (glm::dot(ent->curRotation,rotation) < 0.0f) { ent->prevRotation *= -1.0f; } // set the new position and orientation for this entity ent->curPosition = position; ent->curRotation = rotation; }
void DemoEntity::SetTransformCallback(const NewtonBody* body, const dFloat* matrix, int threadIndex) { DemoEntity* const ent = (DemoEntity*) NewtonBodyGetUserData(body); dQuaternion rot; NewtonBodyGetRotation(body, &rot.m_q0); DemoEntityManager* world = (DemoEntityManager*) NewtonBodyGetWorld(body); const dMatrix& transform = *((dMatrix*) matrix); ent->SetMatrix (*world, rot, transform.m_posit); }
// Transform callback to set the matrix of a the visual entity void SetTransformCallback (const NewtonBody* body, const dFloat* matrix, int threadIndex) { Entity* ent; // Get the position from the matrix dVector posit (matrix[12], matrix[13], matrix[14], 1.0f); dQuaternion rotation; // we will ignore the Rotation part of matrix and use the quaternion rotation stored in the body NewtonBodyGetRotation(body, &rotation.m_q0); // get the entity associated with this rigid body ent = (Entity*) NewtonBodyGetUserData(body); // since this tutorial run the physics and a different fps than the Graphics // we need to save the entity current transformation state before updating the new state. ent->m_prevPosition = ent->m_curPosition; ent->m_prevRotation = ent->m_curRotation; // set the new position and orientation for this entity ent->m_curPosition = posit; ent->m_curRotation = rotation; }
void CustomKinematicController::SubmitConstraints (dFloat timestep, int threadIndex) { // check if this is an impulsive time step if (timestep > 0.0f) { dMatrix matrix0; dVector v(0.0f); dVector w(0.0f); dVector cg(0.0f); dFloat invTimestep = 1.0f / timestep; // calculate the position of the pivot point and the Jacobian direction vectors, in global space. NewtonBodyGetOmega (m_body0, &w[0]); NewtonBodyGetVelocity (m_body0, &v[0]); NewtonBodyGetCentreOfMass (m_body0, &cg[0]); NewtonBodyGetMatrix (m_body0, &matrix0[0][0]); dVector p0 (matrix0.TransformVector (m_localHandle)); dVector pointVeloc (v + w * matrix0.RotateVector (m_localHandle - cg)); dVector relPosit (m_targetPosit - p0); dVector relVeloc (relPosit.Scale (invTimestep) - pointVeloc); dVector relAccel (relVeloc.Scale (invTimestep * 0.3f)); // Restrict the movement on the pivot point along all tree orthonormal direction NewtonUserJointAddLinearRow (m_joint, &p0[0], &m_targetPosit[0], &matrix0.m_front[0]); NewtonUserJointSetRowAcceleration (m_joint, relAccel % matrix0.m_front); NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxLinearFriction); NewtonUserJointSetRowMaximumFriction (m_joint, m_maxLinearFriction); NewtonUserJointAddLinearRow (m_joint, &p0[0], &m_targetPosit[0], &matrix0.m_up[0]); NewtonUserJointSetRowAcceleration (m_joint, relAccel % matrix0.m_up); NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxLinearFriction); NewtonUserJointSetRowMaximumFriction (m_joint, m_maxLinearFriction); NewtonUserJointAddLinearRow (m_joint, &p0[0], &m_targetPosit[0], &matrix0.m_right[0]); NewtonUserJointSetRowAcceleration (m_joint, relAccel % matrix0.m_right); NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxLinearFriction); NewtonUserJointSetRowMaximumFriction (m_joint, m_maxLinearFriction); if (m_pickMode) { dQuaternion rotation; NewtonBodyGetRotation (m_body0, &rotation.m_q0); if (m_targetRot.DotProduct (rotation) < 0.0f) { rotation.m_q0 *= -1.0f; rotation.m_q1 *= -1.0f; rotation.m_q2 *= -1.0f; rotation.m_q3 *= -1.0f; } dVector relOmega (rotation.CalcAverageOmega (m_targetRot, invTimestep) - w); dFloat mag = relOmega % relOmega; if (mag > 1.0e-6f) { dVector pin (relOmega.Scale (1.0f / mag)); dMatrix basis (dGrammSchmidt (pin)); dFloat relSpeed = dSqrt (relOmega % relOmega); dFloat relAlpha = relSpeed * invTimestep; NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis.m_front[0]); NewtonUserJointSetRowAcceleration (m_joint, relAlpha); NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction); NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction); NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis.m_up[0]); NewtonUserJointSetRowAcceleration (m_joint, 0.0f); NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction); NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction); NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis.m_right[0]); NewtonUserJointSetRowAcceleration (m_joint, 0.0f); NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction); NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction); } else { dVector relAlpha (w.Scale (-invTimestep)); NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]); NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_front); NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction); NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction); NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_up[0]); NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_up); NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction); NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction); NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_right[0]); NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_right); NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction); NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction); } } else { // this is the single handle pick mode, add some angular friction dVector relAlpha = w.Scale (-invTimestep); NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]); NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_front); NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction * 0.025f); NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction * 0.025f); NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_up[0]); NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_up); NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction * 0.025f); NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction * 0.025f); NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_right[0]); NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_right); NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction * 0.025f); NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction * 0.025f); } } }
void CustomPlayerController::PostUpdate(dFloat timestep, int threadIndex) { dMatrix matrix; dQuaternion bodyRotation; dVector veloc(0.0f, 0.0f, 0.0f, 0.0f); dVector omega(0.0f, 0.0f, 0.0f, 0.0f); CustomPlayerControllerManager* const manager = (CustomPlayerControllerManager*) GetManager(); NewtonWorld* const world = manager->GetWorld(); // apply the player motion, by calculation the desired plane linear and angular velocity manager->ApplyPlayerMove (this, timestep); // get the body motion state NewtonBodyGetMatrix(m_body, &matrix[0][0]); NewtonBodyGetVelocity(m_body, &veloc[0]); NewtonBodyGetOmega(m_body, &omega[0]); // integrate body angular velocity NewtonBodyGetRotation (m_body, &bodyRotation.m_q0); bodyRotation = bodyRotation.IntegrateOmega(omega, timestep); matrix = dMatrix (bodyRotation, matrix.m_posit); // integrate linear velocity dFloat normalizedTimeLeft = 1.0f; dFloat step = timestep * dSqrt (veloc % veloc) ; dFloat descreteTimeStep = timestep * (1.0f / D_DESCRETE_MOTION_STEPS); int prevContactCount = 0; CustomControllerConvexCastPreFilter castFilterData (m_body); NewtonWorldConvexCastReturnInfo prevInfo[PLAYER_CONTROLLER_MAX_CONTACTS]; dVector updir (matrix.RotateVector(m_upVector)); dVector scale; NewtonCollisionGetScale (m_upperBodyShape, &scale.m_x, &scale.m_y, &scale.m_z); //const dFloat radio = m_outerRadio * 4.0f; const dFloat radio = (m_outerRadio + m_restrainingDistance) * 4.0f; NewtonCollisionSetScale (m_upperBodyShape, m_height - m_stairStep, radio, radio); NewtonWorldConvexCastReturnInfo upConstratint; memset (&upConstratint, 0, sizeof (upConstratint)); upConstratint.m_normal[0] = m_upVector.m_x; upConstratint.m_normal[1] = m_upVector.m_y; upConstratint.m_normal[2] = m_upVector.m_z; upConstratint.m_normal[3] = m_upVector.m_w; for (int j = 0; (j < D_PLAYER_MAX_INTERGRATION_STEPS) && (normalizedTimeLeft > 1.0e-5f); j ++ ) { if ((veloc % veloc) < 1.0e-6f) { break; } dFloat timetoImpact; NewtonWorldConvexCastReturnInfo info[PLAYER_CONTROLLER_MAX_CONTACTS]; dVector destPosit (matrix.m_posit + veloc.Scale (timestep)); int contactCount = NewtonWorldConvexCast (world, &matrix[0][0], &destPosit[0], m_upperBodyShape, &timetoImpact, &castFilterData, CustomControllerConvexCastPreFilter::Prefilter, info, sizeof (info) / sizeof (info[0]), threadIndex); if (contactCount) { contactCount = manager->ProcessContacts (this, info, contactCount); } if (contactCount) { matrix.m_posit += veloc.Scale (timetoImpact * timestep); if (timetoImpact > 0.0f) { matrix.m_posit -= veloc.Scale (D_PLAYER_CONTACT_SKIN_THICKNESS / dSqrt (veloc % veloc)) ; } normalizedTimeLeft -= timetoImpact; dFloat speed[PLAYER_CONTROLLER_MAX_CONTACTS * 2]; dFloat bounceSpeed[PLAYER_CONTROLLER_MAX_CONTACTS * 2]; dVector bounceNormal[PLAYER_CONTROLLER_MAX_CONTACTS * 2]; for (int i = 1; i < contactCount; i ++) { dVector n0 (info[i-1].m_normal); for (int j = 0; j < i; j ++) { dVector n1 (info[j].m_normal); if ((n0 % n1) > 0.9999f) { info[i] = info[contactCount - 1]; i --; contactCount --; break; } } } int count = 0; if (!m_isJumping) { upConstratint.m_point[0] = matrix.m_posit.m_x; upConstratint.m_point[1] = matrix.m_posit.m_y; upConstratint.m_point[2] = matrix.m_posit.m_z; upConstratint.m_point[3] = matrix.m_posit.m_w; speed[count] = 0.0f; bounceNormal[count] = dVector (upConstratint.m_normal); bounceSpeed[count] = CalculateContactKinematics(veloc, &upConstratint); count ++; } for (int i = 0; i < contactCount; i ++) { speed[count] = 0.0f; bounceNormal[count] = dVector (info[i].m_normal); bounceSpeed[count] = CalculateContactKinematics(veloc, &info[i]); count ++; } for (int i = 0; i < prevContactCount; i ++) { speed[count] = 0.0f; bounceNormal[count] = dVector (prevInfo[i].m_normal); bounceSpeed[count] = CalculateContactKinematics(veloc, &prevInfo[i]); count ++; } dFloat residual = 10.0f; dVector auxBounceVeloc (0.0f, 0.0f, 0.0f, 0.0f); for (int i = 0; (i < D_PLAYER_MAX_SOLVER_ITERATIONS) && (residual > 1.0e-3f); i ++) { residual = 0.0f; for (int k = 0; k < count; k ++) { dVector normal (bounceNormal[k]); dFloat v = bounceSpeed[k] - normal % auxBounceVeloc; dFloat x = speed[k] + v; if (x < 0.0f) { v = 0.0f; x = 0.0f; } if (dAbs (v) > residual) { residual = dAbs (v); } auxBounceVeloc += normal.Scale (x - speed[k]); speed[k] = x; } } dVector velocStep (0.0f, 0.0f, 0.0f, 0.0f); for (int i = 0; i < count; i ++) { dVector normal (bounceNormal[i]); velocStep += normal.Scale (speed[i]); } veloc += velocStep; dFloat velocMag2 = velocStep % velocStep; if (velocMag2 < 1.0e-6f) { dFloat advanceTime = dMin (descreteTimeStep, normalizedTimeLeft * timestep); matrix.m_posit += veloc.Scale (advanceTime); normalizedTimeLeft -= advanceTime / timestep; } prevContactCount = contactCount; memcpy (prevInfo, info, prevContactCount * sizeof (NewtonWorldConvexCastReturnInfo)); } else { matrix.m_posit = destPosit; matrix.m_posit.m_w = 1.0f; break; } } NewtonCollisionSetScale (m_upperBodyShape, scale.m_x, scale.m_y, scale.m_z); // determine if player is standing on some plane dMatrix supportMatrix (matrix); supportMatrix.m_posit += updir.Scale (m_sphereCastOrigin); if (m_isJumping) { dVector dst (matrix.m_posit); UpdateGroundPlane (matrix, supportMatrix, dst, threadIndex); } else { step = dAbs (updir % veloc.Scale (timestep)); dFloat castDist = ((m_groundPlane % m_groundPlane) > 0.0f) ? m_stairStep : step; dVector dst (matrix.m_posit - updir.Scale (castDist * 2.0f)); UpdateGroundPlane (matrix, supportMatrix, dst, threadIndex); } // set player velocity, position and orientation NewtonBodySetVelocity(m_body, &veloc[0]); NewtonBodySetMatrix (m_body, &matrix[0][0]); }