void IK_SetTransform(IK_Segment *seg, float start[3], float rest[][3], float basis[][3], float length) { IK_QSegment *qseg = (IK_QSegment*)seg; MT_Vector3 mstart(start); // convert from blender column major to moto row major MT_Matrix3x3 mbasis(basis[0][0], basis[1][0], basis[2][0], basis[0][1], basis[1][1], basis[2][1], basis[0][2], basis[1][2], basis[2][2]); MT_Matrix3x3 mrest(rest[0][0], rest[1][0], rest[2][0], rest[0][1], rest[1][1], rest[2][1], rest[0][2], rest[1][2], rest[2][2]); MT_Scalar mlength(length); if (qseg->Composite()) { MT_Vector3 cstart(0, 0, 0); MT_Matrix3x3 cbasis; cbasis.setIdentity(); qseg->SetTransform(mstart, mrest, mbasis, 0.0); qseg->Composite()->SetTransform(cstart, cbasis, cbasis, mlength); } else qseg->SetTransform(mstart, mrest, mbasis, mlength); }
bool PyOrientationTo(PyObject *pyval, MT_Matrix3x3 &rot, const char *error_prefix) { int size= PySequence_Size(pyval); if (size == 4) { MT_Quaternion qrot; if (PyQuatTo(pyval, qrot)) { rot.setRotation(qrot); return true; } } else if (size == 3) { /* 3x3 matrix or euler */ MT_Vector3 erot; if (PyVecTo(pyval, erot)) { rot.setEuler(erot); return true; } PyErr_Clear(); if (PyMatTo(pyval, rot)) { return true; } } PyErr_Format(PyExc_TypeError, "%s, could not set the orientation from a 3x3 matrix, quaternion or euler sequence", error_prefix); return false; }
MT_Point3 KX_NavMeshObject::TransformToWorldCoords(const MT_Point3& lpos) { MT_Matrix3x3 orientation = NodeGetWorldOrientation(); const MT_Vector3& scaling = NodeGetWorldScaling(); orientation.scale(scaling[0], scaling[1], scaling[2]); MT_Transform worldtr(NodeGetWorldPosition(), orientation); MT_Point3 wpos = worldtr(lpos); return wpos; }
MT_Point3 KX_NavMeshObject::TransformToLocalCoords(const MT_Point3& wpos) { MT_Matrix3x3 orientation = NodeGetWorldOrientation(); const MT_Vector3& scaling = NodeGetWorldScaling(); orientation.scale(scaling[0], scaling[1], scaling[2]); MT_Transform worldtr(NodeGetWorldPosition(), orientation); MT_Transform invworldtr; invworldtr.invert(worldtr); MT_Point3 lpos = invworldtr(wpos); return lpos; }
static void RemoveTwist(MT_Matrix3x3& R) { // compute twist parameter MT_Scalar tau = ComputeTwist(R); // compute twist matrix MT_Matrix3x3 T = ComputeTwistMatrix(tau); // remove twist R = R*T.transposed(); }
void KX_KetsjiEngine::DoSound(KX_Scene* scene) { m_logger->StartLog(tc_sound, m_kxsystem->GetTimeInSeconds(), true); KX_Camera* cam = scene->GetActiveCamera(); if (!cam) return; MT_Point3 listenerposition = cam->NodeGetWorldPosition(); MT_Vector3 listenervelocity = cam->GetLinearVelocity(); MT_Matrix3x3 listenerorientation = cam->NodeGetWorldOrientation(); { AUD_3DData data; float f; listenerorientation.getValue3x3(data.orientation); listenerposition.getValue(data.position); listenervelocity.getValue(data.velocity); f = data.position[1]; data.position[1] = data.position[2]; data.position[2] = -f; f = data.velocity[1]; data.velocity[1] = data.velocity[2]; data.velocity[2] = -f; f = data.orientation[1]; data.orientation[1] = data.orientation[2]; data.orientation[2] = -f; f = data.orientation[3]; data.orientation[3] = -data.orientation[6]; data.orientation[6] = f; f = data.orientation[4]; data.orientation[4] = -data.orientation[8]; data.orientation[8] = -f; f = data.orientation[5]; data.orientation[5] = data.orientation[7]; data.orientation[7] = f; AUD_updateListener(&data); } }
/* vectomat function obtained from constrain.c and modified to work with MOTO library */ static MT_Matrix3x3 vectomat(MT_Vector3 vec, short axis, short upflag, short threedimup) { MT_Matrix3x3 mat; MT_Vector3 y(MT_Scalar(0.0f), MT_Scalar(1.0f), MT_Scalar(0.0f)); MT_Vector3 z(MT_Scalar(0.0f), MT_Scalar(0.0f), MT_Scalar(1.0f)); /* world Z axis is the global up axis */ MT_Vector3 proj; MT_Vector3 right; MT_Scalar mul; int right_index; /* Normalized Vec vector*/ vec = vec.safe_normalized_vec(z); /* if 2D doesn't move the up vector */ if (!threedimup) { vec.setValue(MT_Scalar(vec[0]), MT_Scalar(vec[1]), MT_Scalar(0.0f)); vec = (vec - z.dot(vec)*z).safe_normalized_vec(z); } if (axis > 2) axis -= 3; else vec = -vec; /* project the up vector onto the plane specified by vec */ /* first z onto vec... */ mul = z.dot(vec) / vec.dot(vec); proj = vec * mul; /* then onto the plane */ proj = z - proj; /* proj specifies the transformation of the up axis */ proj = proj.safe_normalized_vec(y); /* Normalized cross product of vec and proj specifies transformation of the right axis */ right = proj.cross(vec); right.normalize(); if (axis != upflag) { right_index = 3 - axis - upflag; /* account for up direction, track direction */ right = right * basis_cross(axis, upflag); mat.setRow(right_index, right); mat.setRow(upflag, proj); mat.setRow(axis, vec); mat = mat.inverse(); } /* identity matrix - don't do anything if the two axes are the same */ else { mat.setIdentity(); } return mat; }
void RAS_OpenGLRasterizer::FlushDebugShapes() { if (m_debugShapes.empty()) return; // DrawDebugLines GLboolean light, tex; light= glIsEnabled(GL_LIGHTING); tex= glIsEnabled(GL_TEXTURE_2D); if (light) glDisable(GL_LIGHTING); if (tex) glDisable(GL_TEXTURE_2D); //draw lines glBegin(GL_LINES); for (unsigned int i=0;i<m_debugShapes.size();i++) { if (m_debugShapes[i].m_type != OglDebugShape::LINE) continue; glColor4f(m_debugShapes[i].m_color[0],m_debugShapes[i].m_color[1],m_debugShapes[i].m_color[2],1.f); const MT_Scalar* fromPtr = &m_debugShapes[i].m_pos.x(); const MT_Scalar* toPtr= &m_debugShapes[i].m_param.x(); glVertex3dv(fromPtr); glVertex3dv(toPtr); } glEnd(); //draw circles for (unsigned int i=0;i<m_debugShapes.size();i++) { if (m_debugShapes[i].m_type != OglDebugShape::CIRCLE) continue; glBegin(GL_LINE_LOOP); glColor4f(m_debugShapes[i].m_color[0],m_debugShapes[i].m_color[1],m_debugShapes[i].m_color[2],1.f); static const MT_Vector3 worldUp(0.0, 0.0, 1.0); MT_Vector3 norm = m_debugShapes[i].m_param; MT_Matrix3x3 tr; if (norm.fuzzyZero() || norm == worldUp) { tr.setIdentity(); } else { MT_Vector3 xaxis, yaxis; xaxis = MT_cross(norm, worldUp); yaxis = MT_cross(xaxis, norm); tr.setValue(xaxis.x(), xaxis.y(), xaxis.z(), yaxis.x(), yaxis.y(), yaxis.z(), norm.x(), norm.y(), norm.z()); } MT_Scalar rad = m_debugShapes[i].m_param2.x(); int n = (int) m_debugShapes[i].m_param2.y(); for (int j = 0; j<n; j++) { MT_Scalar theta = j*M_PI*2/n; MT_Vector3 pos(cos(theta) * rad, sin(theta) * rad, 0.0); pos = pos*tr; pos += m_debugShapes[i].m_pos; const MT_Scalar* posPtr = &pos.x(); glVertex3dv(posPtr); } glEnd(); } if (light) glEnable(GL_LIGHTING); if (tex) glEnable(GL_TEXTURE_2D); m_debugShapes.clear(); }
void KX_BulletPhysicsController::RelativeRotate(const MT_Matrix3x3& drot,bool local) { float rotval[9]; drot.getValue3x3(rotval); CcdPhysicsController::RelativeRotate(rotval,local); }
void KX_ConvertBulletObject( class KX_GameObject* gameobj, class RAS_MeshObject* meshobj, struct DerivedMesh* dm, class KX_Scene* kxscene, struct PHY_ShapeProps* shapeprops, struct PHY_MaterialProps* smmaterial, struct KX_ObjectProperties* objprop) { CcdPhysicsEnvironment* env = (CcdPhysicsEnvironment*)kxscene->GetPhysicsEnvironment(); assert(env); bool isbulletdyna = false; bool isbulletsensor = false; bool isbulletchar = false; bool useGimpact = false; CcdConstructionInfo ci; class PHY_IMotionState* motionstate = new KX_MotionState(gameobj->GetSGNode()); class CcdShapeConstructionInfo *shapeInfo = new CcdShapeConstructionInfo(); if (!objprop->m_dyna) { ci.m_collisionFlags |= btCollisionObject::CF_STATIC_OBJECT; } if (objprop->m_ghost) { ci.m_collisionFlags |= btCollisionObject::CF_NO_CONTACT_RESPONSE; } ci.m_MotionState = motionstate; ci.m_gravity = btVector3(0,0,0); ci.m_linearFactor = btVector3(objprop->m_lockXaxis? 0 : 1, objprop->m_lockYaxis? 0 : 1, objprop->m_lockZaxis? 0 : 1); ci.m_angularFactor = btVector3(objprop->m_lockXRotaxis? 0 : 1, objprop->m_lockYRotaxis? 0 : 1, objprop->m_lockZRotaxis? 0 : 1); ci.m_localInertiaTensor =btVector3(0,0,0); ci.m_mass = objprop->m_dyna ? shapeprops->m_mass : 0.f; ci.m_clamp_vel_min = shapeprops->m_clamp_vel_min; ci.m_clamp_vel_max = shapeprops->m_clamp_vel_max; ci.m_margin = objprop->m_margin; ci.m_stepHeight = objprop->m_character ? shapeprops->m_step_height : 0.f; ci.m_jumpSpeed = objprop->m_character ? shapeprops->m_jump_speed : 0.f; ci.m_fallSpeed = objprop->m_character ? shapeprops->m_fall_speed : 0.f; shapeInfo->m_radius = objprop->m_radius; isbulletdyna = objprop->m_dyna; isbulletsensor = objprop->m_sensor; isbulletchar = objprop->m_character; useGimpact = ((isbulletdyna || isbulletsensor) && !objprop->m_softbody); ci.m_localInertiaTensor = btVector3(ci.m_mass/3.f,ci.m_mass/3.f,ci.m_mass/3.f); btCollisionShape* bm = 0; switch (objprop->m_boundclass) { case KX_BOUNDSPHERE: { //float radius = objprop->m_radius; //btVector3 inertiaHalfExtents ( // radius, // radius, // radius); //blender doesn't support multisphere, but for testing: //bm = new MultiSphereShape(inertiaHalfExtents,,&trans.getOrigin(),&radius,1); shapeInfo->m_shapeType = PHY_SHAPE_SPHERE; bm = shapeInfo->CreateBulletShape(ci.m_margin); break; }; case KX_BOUNDBOX: { shapeInfo->m_halfExtend.setValue( objprop->m_boundobject.box.m_extends[0], objprop->m_boundobject.box.m_extends[1], objprop->m_boundobject.box.m_extends[2]); shapeInfo->m_halfExtend /= 2.0; shapeInfo->m_halfExtend = shapeInfo->m_halfExtend.absolute(); shapeInfo->m_shapeType = PHY_SHAPE_BOX; bm = shapeInfo->CreateBulletShape(ci.m_margin); break; }; case KX_BOUNDCYLINDER: { shapeInfo->m_halfExtend.setValue( objprop->m_boundobject.c.m_radius, objprop->m_boundobject.c.m_radius, objprop->m_boundobject.c.m_height * 0.5f ); shapeInfo->m_shapeType = PHY_SHAPE_CYLINDER; bm = shapeInfo->CreateBulletShape(ci.m_margin); break; } case KX_BOUNDCONE: { shapeInfo->m_radius = objprop->m_boundobject.c.m_radius; shapeInfo->m_height = objprop->m_boundobject.c.m_height; shapeInfo->m_shapeType = PHY_SHAPE_CONE; bm = shapeInfo->CreateBulletShape(ci.m_margin); break; } case KX_BOUNDPOLYTOPE: { shapeInfo->SetMesh(meshobj, dm,true); bm = shapeInfo->CreateBulletShape(ci.m_margin); break; } case KX_BOUNDCAPSULE: { shapeInfo->m_radius = objprop->m_boundobject.c.m_radius; shapeInfo->m_height = objprop->m_boundobject.c.m_height; shapeInfo->m_shapeType = PHY_SHAPE_CAPSULE; bm = shapeInfo->CreateBulletShape(ci.m_margin); break; } case KX_BOUNDMESH: { // mesh shapes can be shared, check first if we already have a shape on that mesh class CcdShapeConstructionInfo *sharedShapeInfo = CcdShapeConstructionInfo::FindMesh(meshobj, dm, false); if (sharedShapeInfo != NULL) { shapeInfo->Release(); shapeInfo = sharedShapeInfo; shapeInfo->AddRef(); } else { shapeInfo->SetMesh(meshobj, dm, false); } // Soft bodies can benefit from welding, don't do it on non-soft bodies if (objprop->m_softbody) { shapeInfo->setVertexWeldingThreshold1(objprop->m_soft_welding); //todo: expose this to the UI } bm = shapeInfo->CreateBulletShape(ci.m_margin, useGimpact, !objprop->m_softbody); //should we compute inertia for dynamic shape? //bm->calculateLocalInertia(ci.m_mass,ci.m_localInertiaTensor); break; } case KX_BOUND_DYN_MESH: /* do nothing */ break; } // ci.m_localInertiaTensor.setValue(0.1f,0.1f,0.1f); if (!bm) { delete motionstate; shapeInfo->Release(); return; } //bm->setMargin(ci.m_margin); if (objprop->m_isCompoundChild) { //find parent, compound shape and add to it //take relative transform into account! CcdPhysicsController* parentCtrl = (CcdPhysicsController*)objprop->m_dynamic_parent->GetPhysicsController(); assert(parentCtrl); CcdShapeConstructionInfo* parentShapeInfo = parentCtrl->GetShapeInfo(); btRigidBody* rigidbody = parentCtrl->GetRigidBody(); btCollisionShape* colShape = rigidbody->getCollisionShape(); assert(colShape->isCompound()); btCompoundShape* compoundShape = (btCompoundShape*)colShape; // compute the local transform from parent, this may include several node in the chain SG_Node* gameNode = gameobj->GetSGNode(); SG_Node* parentNode = objprop->m_dynamic_parent->GetSGNode(); // relative transform MT_Vector3 parentScale = parentNode->GetWorldScaling(); parentScale[0] = MT_Scalar(1.0)/parentScale[0]; parentScale[1] = MT_Scalar(1.0)/parentScale[1]; parentScale[2] = MT_Scalar(1.0)/parentScale[2]; MT_Vector3 relativeScale = gameNode->GetWorldScaling() * parentScale; MT_Matrix3x3 parentInvRot = parentNode->GetWorldOrientation().transposed(); MT_Vector3 relativePos = parentInvRot*((gameNode->GetWorldPosition()-parentNode->GetWorldPosition())*parentScale); MT_Matrix3x3 relativeRot = parentInvRot*gameNode->GetWorldOrientation(); shapeInfo->m_childScale.setValue(relativeScale[0],relativeScale[1],relativeScale[2]); bm->setLocalScaling(shapeInfo->m_childScale); shapeInfo->m_childTrans.getOrigin().setValue(relativePos[0],relativePos[1],relativePos[2]); float rot[12]; relativeRot.getValue(rot); shapeInfo->m_childTrans.getBasis().setFromOpenGLSubMatrix(rot); parentShapeInfo->AddShape(shapeInfo); compoundShape->addChildShape(shapeInfo->m_childTrans,bm); //do some recalc? //recalc inertia for rigidbody if (!rigidbody->isStaticOrKinematicObject()) { btVector3 localInertia; float mass = 1.f/rigidbody->getInvMass(); compoundShape->calculateLocalInertia(mass,localInertia); rigidbody->setMassProps(mass,localInertia); } shapeInfo->Release(); // delete motionstate as it's not used delete motionstate; return; } if (objprop->m_hasCompoundChildren) { // create a compound shape info CcdShapeConstructionInfo *compoundShapeInfo = new CcdShapeConstructionInfo(); compoundShapeInfo->m_shapeType = PHY_SHAPE_COMPOUND; compoundShapeInfo->AddShape(shapeInfo); // create the compound shape manually as we already have the child shape btCompoundShape* compoundShape = new btCompoundShape(); compoundShape->addChildShape(shapeInfo->m_childTrans,bm); // now replace the shape bm = compoundShape; shapeInfo->Release(); shapeInfo = compoundShapeInfo; } #ifdef TEST_SIMD_HULL if (bm->IsPolyhedral()) { PolyhedralConvexShape* polyhedron = static_cast<PolyhedralConvexShape*>(bm); if (!polyhedron->m_optionalHull) { //first convert vertices in 'Point3' format int numPoints = polyhedron->GetNumVertices(); Point3* points = new Point3[numPoints+1]; //first 4 points should not be co-planar, so add central point to satisfy MakeHull points[0] = Point3(0.f,0.f,0.f); btVector3 vertex; for (int p=0;p<numPoints;p++) { polyhedron->GetVertex(p,vertex); points[p+1] = Point3(vertex.getX(),vertex.getY(),vertex.getZ()); } Hull* hull = Hull::MakeHull(numPoints+1,points); polyhedron->m_optionalHull = hull; } } #endif //TEST_SIMD_HULL ci.m_collisionShape = bm; ci.m_shapeInfo = shapeInfo; ci.m_friction = smmaterial->m_friction;//tweak the friction a bit, so the default 0.5 works nice ci.m_restitution = smmaterial->m_restitution; ci.m_physicsEnv = env; // drag / damping is inverted ci.m_linearDamping = 1.f - shapeprops->m_lin_drag; ci.m_angularDamping = 1.f - shapeprops->m_ang_drag; //need a bit of damping, else system doesn't behave well ci.m_inertiaFactor = shapeprops->m_inertia/0.4f;//defaults to 0.4, don't want to change behavior ci.m_do_anisotropic = shapeprops->m_do_anisotropic; ci.m_anisotropicFriction.setValue(shapeprops->m_friction_scaling[0],shapeprops->m_friction_scaling[1],shapeprops->m_friction_scaling[2]); ////////// //do Fh, do Rot Fh ci.m_do_fh = shapeprops->m_do_fh; ci.m_do_rot_fh = shapeprops->m_do_rot_fh; ci.m_fh_damping = smmaterial->m_fh_damping; ci.m_fh_distance = smmaterial->m_fh_distance; ci.m_fh_normal = smmaterial->m_fh_normal; ci.m_fh_spring = smmaterial->m_fh_spring; ci.m_radius = objprop->m_radius; /////////////////// ci.m_gamesoftFlag = objprop->m_gamesoftFlag; ci.m_soft_linStiff = objprop->m_soft_linStiff; ci.m_soft_angStiff = objprop->m_soft_angStiff; /* angular stiffness 0..1 */ ci.m_soft_volume= objprop->m_soft_volume; /* volume preservation 0..1 */ ci.m_soft_viterations= objprop->m_soft_viterations; /* Velocities solver iterations */ ci.m_soft_piterations= objprop->m_soft_piterations; /* Positions solver iterations */ ci.m_soft_diterations= objprop->m_soft_diterations; /* Drift solver iterations */ ci.m_soft_citerations= objprop->m_soft_citerations; /* Cluster solver iterations */ ci.m_soft_kSRHR_CL= objprop->m_soft_kSRHR_CL; /* Soft vs rigid hardness [0,1] (cluster only) */ ci.m_soft_kSKHR_CL= objprop->m_soft_kSKHR_CL; /* Soft vs kinetic hardness [0,1] (cluster only) */ ci.m_soft_kSSHR_CL= objprop->m_soft_kSSHR_CL; /* Soft vs soft hardness [0,1] (cluster only) */ ci.m_soft_kSR_SPLT_CL= objprop->m_soft_kSR_SPLT_CL; /* Soft vs rigid impulse split [0,1] (cluster only) */ ci.m_soft_kSK_SPLT_CL= objprop->m_soft_kSK_SPLT_CL; /* Soft vs rigid impulse split [0,1] (cluster only) */ ci.m_soft_kSS_SPLT_CL= objprop->m_soft_kSS_SPLT_CL; /* Soft vs rigid impulse split [0,1] (cluster only) */ ci.m_soft_kVCF= objprop->m_soft_kVCF; /* Velocities correction factor (Baumgarte) */ ci.m_soft_kDP= objprop->m_soft_kDP; /* Damping coefficient [0,1] */ ci.m_soft_kDG= objprop->m_soft_kDG; /* Drag coefficient [0,+inf] */ ci.m_soft_kLF= objprop->m_soft_kLF; /* Lift coefficient [0,+inf] */ ci.m_soft_kPR= objprop->m_soft_kPR; /* Pressure coefficient [-inf,+inf] */ ci.m_soft_kVC= objprop->m_soft_kVC; /* Volume conversation coefficient [0,+inf] */ ci.m_soft_kDF= objprop->m_soft_kDF; /* Dynamic friction coefficient [0,1] */ ci.m_soft_kMT= objprop->m_soft_kMT; /* Pose matching coefficient [0,1] */ ci.m_soft_kCHR= objprop->m_soft_kCHR; /* Rigid contacts hardness [0,1] */ ci.m_soft_kKHR= objprop->m_soft_kKHR; /* Kinetic contacts hardness [0,1] */ ci.m_soft_kSHR= objprop->m_soft_kSHR; /* Soft contacts hardness [0,1] */ ci.m_soft_kAHR= objprop->m_soft_kAHR; /* Anchors hardness [0,1] */ ci.m_soft_collisionflags= objprop->m_soft_collisionflags; /* Vertex/Face or Signed Distance Field(SDF) or Clusters, Soft versus Soft or Rigid */ ci.m_soft_numclusteriterations= objprop->m_soft_numclusteriterations; /* number of iterations to refine collision clusters*/ //////////////////// ci.m_collisionFilterGroup = (isbulletsensor) ? short(CcdConstructionInfo::SensorFilter) : (isbulletdyna) ? short(CcdConstructionInfo::DefaultFilter) : (isbulletchar) ? short(CcdConstructionInfo::CharacterFilter) : short(CcdConstructionInfo::StaticFilter); ci.m_collisionFilterMask = (isbulletsensor) ? short(CcdConstructionInfo::AllFilter ^ CcdConstructionInfo::SensorFilter) : (isbulletdyna) ? short(CcdConstructionInfo::AllFilter) : (isbulletchar) ? short(CcdConstructionInfo::AllFilter) : short(CcdConstructionInfo::AllFilter ^ CcdConstructionInfo::StaticFilter); ci.m_bRigid = objprop->m_dyna && objprop->m_angular_rigidbody; ci.m_contactProcessingThreshold = objprop->m_contactProcessingThreshold;//todo: expose this in advanced settings, just like margin, default to 10000 or so ci.m_bSoft = objprop->m_softbody; ci.m_bDyna = isbulletdyna; ci.m_bSensor = isbulletsensor; ci.m_bCharacter = isbulletchar; ci.m_bGimpact = useGimpact; MT_Vector3 scaling = gameobj->NodeGetWorldScaling(); ci.m_scaling.setValue(scaling[0], scaling[1], scaling[2]); CcdPhysicsController* physicscontroller = new CcdPhysicsController(ci); // shapeInfo is reference counted, decrement now as we don't use it anymore if (shapeInfo) shapeInfo->Release(); gameobj->SetPhysicsController(physicscontroller,isbulletdyna); // don't add automatically sensor object, they are added when a collision sensor is registered if (!isbulletsensor && objprop->m_in_active_layer) { env->AddCcdPhysicsController( physicscontroller); } physicscontroller->SetNewClientInfo(gameobj->getClientInfo()); { btRigidBody* rbody = physicscontroller->GetRigidBody(); if (rbody) { if (objprop->m_angular_rigidbody) { rbody->setLinearFactor(ci.m_linearFactor); rbody->setAngularFactor(ci.m_angularFactor); } if (rbody && objprop->m_disableSleeping) { rbody->setActivationState(DISABLE_DEACTIVATION); } } } CcdPhysicsController* parentCtrl = objprop->m_dynamic_parent ? (CcdPhysicsController*)objprop->m_dynamic_parent->GetPhysicsController() : 0; physicscontroller->SetParentCtrl(parentCtrl); //Now done directly in ci.m_collisionFlags so that it propagates to replica //if (objprop->m_ghost) //{ // rbody->setCollisionFlags(rbody->getCollisionFlags() | btCollisionObject::CF_NO_CONTACT_RESPONSE); //} if (objprop->m_dyna && !objprop->m_angular_rigidbody) { #if 0 //setting the inertia could achieve similar results to constraint the up //but it is prone to instability, so use special 'Angular' constraint btVector3 inertia = physicscontroller->GetRigidBody()->getInvInertiaDiagLocal(); inertia.setX(0.f); inertia.setZ(0.f); physicscontroller->GetRigidBody()->setInvInertiaDiagLocal(inertia); physicscontroller->GetRigidBody()->updateInertiaTensor(); #endif //env->createConstraint(physicscontroller,0,PHY_ANGULAR_CONSTRAINT,0,0,0,0,0,1); //Now done directly in ci.m_bRigid so that it propagates to replica //physicscontroller->GetRigidBody()->setAngularFactor(0.f); ; } bool isActor = objprop->m_isactor; gameobj->getClientInfo()->m_type = (isbulletsensor) ? ((isActor) ? KX_ClientObjectInfo::OBACTORSENSOR : KX_ClientObjectInfo::OBSENSOR) : (isActor) ? KX_ClientObjectInfo::ACTOR : KX_ClientObjectInfo::STATIC; // store materialname in auxinfo, needed for touchsensors if (meshobj) { const STR_String& matname=meshobj->GetMaterialName(0); gameobj->getClientInfo()->m_auxilary_info = (matname.Length() ? (void*)(matname.ReadPtr()+2) : NULL); } else { gameobj->getClientInfo()->m_auxilary_info = 0; } STR_String materialname; if (meshobj) materialname = meshobj->GetMaterialName(0); #if 0 ///test for soft bodies if (objprop->m_softbody && physicscontroller) { btSoftBody* softBody = physicscontroller->GetSoftBody(); if (softBody && gameobj->GetMesh(0))//only the first mesh, if any { //should be a mesh then, so add a soft body deformer KX_SoftBodyDeformer* softbodyDeformer = new KX_SoftBodyDeformer( gameobj->GetMesh(0),(BL_DeformableGameObject*)gameobj); gameobj->SetDeformer(softbodyDeformer); } } #endif }
bool KX_RaySensor::Evaluate() { bool result = false; bool reset = m_reset && m_level; m_rayHit = false; m_hitObject = NULL; m_hitPosition[0] = 0; m_hitPosition[1] = 0; m_hitPosition[2] = 0; m_hitNormal[0] = 1; m_hitNormal[1] = 0; m_hitNormal[2] = 0; KX_GameObject* obj = (KX_GameObject*)GetParent(); MT_Point3 frompoint = obj->NodeGetWorldPosition(); MT_Matrix3x3 matje = obj->NodeGetWorldOrientation(); MT_Matrix3x3 invmat = matje.inverse(); MT_Vector3 todir; m_reset = false; switch (m_axis) { case SENS_RAY_X_AXIS: // X { todir[0] = invmat[0][0]; todir[1] = invmat[0][1]; todir[2] = invmat[0][2]; break; } case SENS_RAY_Y_AXIS: // Y { todir[0] = invmat[1][0]; todir[1] = invmat[1][1]; todir[2] = invmat[1][2]; break; } case SENS_RAY_Z_AXIS: // Z { todir[0] = invmat[2][0]; todir[1] = invmat[2][1]; todir[2] = invmat[2][2]; break; } case SENS_RAY_NEG_X_AXIS: // -X { todir[0] = -invmat[0][0]; todir[1] = -invmat[0][1]; todir[2] = -invmat[0][2]; break; } case SENS_RAY_NEG_Y_AXIS: // -Y { todir[0] = -invmat[1][0]; todir[1] = -invmat[1][1]; todir[2] = -invmat[1][2]; break; } case SENS_RAY_NEG_Z_AXIS: // -Z { todir[0] = -invmat[2][0]; todir[1] = -invmat[2][1]; todir[2] = -invmat[2][2]; break; } } todir.normalize(); m_rayDirection[0] = todir[0]; m_rayDirection[1] = todir[1]; m_rayDirection[2] = todir[2]; MT_Point3 topoint = frompoint + (m_distance) * todir; PHY_IPhysicsEnvironment* pe = m_scene->GetPhysicsEnvironment(); if (!pe) { std::cout << "WARNING: Ray sensor " << GetName() << ": There is no physics environment!" << std::endl; std::cout << " Check universe for malfunction." << std::endl; return false; } KX_IPhysicsController *spc = obj->GetPhysicsController(); KX_GameObject *parent = obj->GetParent(); if (!spc && parent) spc = parent->GetPhysicsController(); if (parent) parent->Release(); PHY_IPhysicsEnvironment* physics_environment = this->m_scene->GetPhysicsEnvironment(); KX_RayCast::Callback<KX_RaySensor> callback(this, spc); KX_RayCast::RayTest(physics_environment, frompoint, topoint, callback); /* now pass this result to some controller */ if (m_rayHit) { if (!m_bTriggered) { // notify logicsystem that ray is now hitting result = true; m_bTriggered = true; } else { // notify logicsystem that ray is STILL hitting ... result = false; } } else { if (m_bTriggered) { m_bTriggered = false; // notify logicsystem that ray JUST left the Object result = true; } else { result = false; } } if (reset) // force an event result = true; return result; }
void KX_SteeringActuator::HandleActorFace(MT_Vector3& velocity) { if (m_facingMode==0 && (!m_navmesh || !m_normalUp)) return; KX_GameObject* curobj = (KX_GameObject*) GetParent(); MT_Vector3 dir = m_facingMode==0 ? curobj->NodeGetLocalOrientation().getColumn(1) : velocity; if (dir.fuzzyZero()) return; dir.normalize(); MT_Vector3 up(0,0,1); MT_Vector3 left; MT_Matrix3x3 mat; if (m_navmesh && m_normalUp) { dtStatNavMesh* navmesh = m_navmesh->GetNavMesh(); MT_Vector3 normal; MT_Vector3 trpos = m_navmesh->TransformToLocalCoords(curobj->NodeGetWorldPosition()); if (getNavmeshNormal(navmesh, trpos, normal)) { left = (dir.cross(up)).safe_normalized(); dir = (-left.cross(normal)).safe_normalized(); up = normal; } } switch (m_facingMode) { case 1: // TRACK X { left = dir.safe_normalized(); dir = -(left.cross(up)).safe_normalized(); break; }; case 2: // TRACK Y { left = (dir.cross(up)).safe_normalized(); break; } case 3: // track Z { left = up.safe_normalized(); up = dir.safe_normalized(); dir = left; left = (dir.cross(up)).safe_normalized(); break; } case 4: // TRACK -X { left = -dir.safe_normalized(); dir = -(left.cross(up)).safe_normalized(); break; }; case 5: // TRACK -Y { left = (-dir.cross(up)).safe_normalized(); dir = -dir; break; } case 6: // track -Z { left = up.safe_normalized(); up = -dir.safe_normalized(); dir = left; left = (dir.cross(up)).safe_normalized(); break; } } mat.setValue ( left[0], dir[0],up[0], left[1], dir[1],up[1], left[2], dir[2],up[2] ); KX_GameObject* parentObject = curobj->GetParent(); if (parentObject) { MT_Vector3 localpos; localpos = curobj->GetSGNode()->GetLocalPosition(); MT_Matrix3x3 parentmatinv; parentmatinv = parentObject->NodeGetWorldOrientation ().inverse (); mat = parentmatinv * mat; mat = m_parentlocalmat * mat; curobj->NodeSetLocalOrientation(mat); curobj->NodeSetLocalPosition(localpos); } else { curobj->NodeSetLocalOrientation(mat); } }
bool KX_TrackToActuator::Update(double curtime, bool frame) { bool result = false; bool bNegativeEvent = IsNegativeEvent(); RemoveAllEvents(); if (bNegativeEvent) { // do nothing on negative events } else if (m_object) { KX_GameObject* curobj = (KX_GameObject*) GetParent(); MT_Vector3 dir = ((KX_GameObject*)m_object)->NodeGetWorldPosition() - curobj->NodeGetWorldPosition(); if (dir.length2()) dir.normalize(); MT_Vector3 up(0,0,1); #ifdef DSADSA switch (m_upflag) { case 0: { up.setValue(1.0,0,0); break; } case 1: { up.setValue(0,1.0,0); break; } case 2: default: { up.setValue(0,0,1.0); } } #endif if (m_allow3D) { up = (up - up.dot(dir) * dir).safe_normalized(); } else { dir = (dir - up.dot(dir)*up).safe_normalized(); } MT_Vector3 left; MT_Matrix3x3 mat; switch (m_trackflag) { case 0: // TRACK X { // (1.0 , 0.0 , 0.0 ) x direction is forward, z (0.0 , 0.0 , 1.0 ) up left = dir.safe_normalized(); dir = (left.cross(up)).safe_normalized(); mat.setValue ( left[0], dir[0],up[0], left[1], dir[1],up[1], left[2], dir[2],up[2] ); break; }; case 1: // TRACK Y { // (0.0 , 1.0 , 0.0 ) y direction is forward, z (0.0 , 0.0 , 1.0 ) up left = (dir.cross(up)).safe_normalized(); mat.setValue ( left[0], dir[0],up[0], left[1], dir[1],up[1], left[2], dir[2],up[2] ); break; } case 2: // track Z { left = up.safe_normalized(); up = dir.safe_normalized(); dir = left; left = (dir.cross(up)).safe_normalized(); mat.setValue ( left[0], dir[0],up[0], left[1], dir[1],up[1], left[2], dir[2],up[2] ); break; } case 3: // TRACK -X { // (1.0 , 0.0 , 0.0 ) x direction is forward, z (0.0 , 0.0 , 1.0 ) up left = -dir.safe_normalized(); dir = -(left.cross(up)).safe_normalized(); mat.setValue ( left[0], dir[0],up[0], left[1], dir[1],up[1], left[2], dir[2],up[2] ); break; }; case 4: // TRACK -Y { // (0.0 , -1.0 , 0.0 ) -y direction is forward, z (0.0 , 0.0 , 1.0 ) up left = (-dir.cross(up)).safe_normalized(); mat.setValue ( left[0], -dir[0],up[0], left[1], -dir[1],up[1], left[2], -dir[2],up[2] ); break; } case 5: // track -Z { left = up.safe_normalized(); up = -dir.safe_normalized(); dir = left; left = (dir.cross(up)).safe_normalized(); mat.setValue ( left[0], dir[0],up[0], left[1], dir[1],up[1], left[2], dir[2],up[2] ); break; } default: { // (1.0 , 0.0 , 0.0 ) -x direction is forward, z (0.0 , 0.0 , 1.0 ) up left = -dir.safe_normalized(); dir = -(left.cross(up)).safe_normalized(); mat.setValue ( left[0], dir[0],up[0], left[1], dir[1],up[1], left[2], dir[2],up[2] ); } } MT_Matrix3x3 oldmat; oldmat= curobj->NodeGetWorldOrientation(); /* erwin should rewrite this! */ mat= matrix3x3_interpol(oldmat, mat, m_time); if(m_parentobj){ // check if the model is parented and calculate the child transform MT_Point3 localpos; localpos = curobj->GetSGNode()->GetLocalPosition(); // Get the inverse of the parent matrix MT_Matrix3x3 parentmatinv; parentmatinv = m_parentobj->NodeGetWorldOrientation ().inverse (); // transform the local coordinate system into the parents system mat = parentmatinv * mat; // append the initial parent local rotation matrix mat = m_parentlocalmat * mat; // set the models tranformation properties curobj->NodeSetLocalOrientation(mat); curobj->NodeSetLocalPosition(localpos); //curobj->UpdateTransform(); } else { curobj->NodeSetLocalOrientation(mat); } result = true; } return result; }