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;
}
