inline MT_Scalar closest_points(const DT_DuoPack<const DT_Convex *, MT_Scalar>& pack, DT_Index a_index, DT_Index b_index, MT_Scalar max_dist2, MT_Point3& pa, MT_Point3& pb)
{
DT_Transform ta = DT_Transform(pack.m_a.m_xform, *pack.m_a.m_leaves[a_index]);
MT_Scalar a_margin = pack.m_a.m_plus;
DT_Transform tb = DT_Transform(pack.m_b.m_xform, *pack.m_b.m_leaves[b_index]);
MT_Scalar b_margin = pack.m_b.m_plus;
return ::closest_points((a_margin > MT_Scalar(0.0) ?
static_cast<const DT_Convex&>(DT_Minkowski(ta, DT_Sphere(a_margin))) :
static_cast<const DT_Convex&>(ta)),
(b_margin > MT_Scalar(0.0) ?
static_cast<const DT_Convex&>(DT_Minkowski(tb, DT_Sphere(b_margin))) :
static_cast<const DT_Convex&>(tb)), max_dist2, pa, pb);
}
inline bool common_point(const DT_DuoPack<const DT_Convex *, MT_Scalar>& pack, DT_Index a_index, DT_Index b_index, MT_Vector3& v, MT_Point3& pa, MT_Point3& pb)
{
DT_Transform ta = DT_Transform(pack.m_a.m_xform, *pack.m_a.m_leaves[a_index]);
MT_Scalar a_margin = pack.m_a.m_plus;
DT_Transform tb = DT_Transform(pack.m_b.m_xform, *pack.m_b.m_leaves[b_index]);
MT_Scalar b_margin = pack.m_b.m_plus;
return ::common_point((a_margin > MT_Scalar(0.0) ?
static_cast<const DT_Convex&>(DT_Minkowski(ta, DT_Sphere(a_margin))) :
static_cast<const DT_Convex&>(ta)),
(b_margin > MT_Scalar(0.0) ?
static_cast<const DT_Convex&>(DT_Minkowski(tb, DT_Sphere(b_margin))) :
static_cast<const DT_Convex&>(tb)),
v, pa, pb);
}
/* 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;
}
bool penetration_depth(const DT_Complex& a, const MT_Transform& a2w, MT_Scalar a_margin,
const DT_Convex& b, MT_Scalar b_margin, MT_Vector3& v, MT_Point3& pa, MT_Point3& pb)
{
DT_HybridPack<const DT_Convex *, MT_Scalar> pack(DT_ObjectData<const DT_Convex *, MT_Scalar>(a.m_nodes, a.m_leaves, a2w, a_margin), b, b_margin);
MT_Scalar max_pen_len = MT_Scalar(0.0);
return penetration_depth(DT_BBoxTree(a.m_cbox + pack.m_a.m_added, 0, a.m_type), pack, v, pa, pb, max_pen_len);
}
void
MT_ExpMap::
angleUpdated(
){
m_theta = m_v.length();
reParametrize();
// compute quaternion, sinp and cosp
if (m_theta < MT_EXPMAP_MINANGLE) {
m_sinp = MT_Scalar(0.0);
/* Taylor Series for sinc */
MT_Vector3 temp = m_v * MT_Scalar(MT_Scalar(.5) - m_theta*m_theta/MT_Scalar(48.0));
m_q.x() = temp.x();
m_q.y() = temp.y();
m_q.z() = temp.z();
m_q.w() = MT_Scalar(1.0);
} else {
m_sinp = MT_Scalar(sin(.5*m_theta));
/* Taylor Series for sinc */
MT_Vector3 temp = m_v * (m_sinp/m_theta);
m_q.x() = temp.x();
m_q.y() = temp.y();
m_q.z() = temp.z();
m_q.w() = MT_Scalar(cos(.5*m_theta));
}
}
inline bool intersect(const DT_Pack<const DT_Convex *, MT_Scalar>& pack, DT_Index a_index, MT_Vector3& v)
{
DT_Transform ta = DT_Transform(pack.m_a.m_xform, *pack.m_a.m_leaves[a_index]);
MT_Scalar a_margin = pack.m_a.m_plus;
return ::intersect((a_margin > MT_Scalar(0.0) ?
static_cast<const DT_Convex&>(DT_Minkowski(ta, DT_Sphere(a_margin))) :
static_cast<const DT_Convex&>(ta)),
pack.m_b, v);
}
void
MT_ExpMap::
reParametrize(
){
if (m_theta > MT_PI) {
MT_Scalar scl = m_theta;
if (m_theta > MT_2_PI){ /* first get theta into range 0..2PI */
m_theta = MT_Scalar(fmod(m_theta, MT_2_PI));
scl = m_theta/scl;
m_v *= scl;
}
if (m_theta > MT_PI){
scl = m_theta;
m_theta = MT_2_PI - m_theta;
scl = MT_Scalar(1.0) - MT_2_PI/scl;
m_v *= scl;
}
}
}
void
MT_ExpMap::
compute_dRdVi(
const MT_Quaternion &dQdvi,
MT_Matrix3x3 & dRdvi
) const {
MT_Scalar prod[9];
/* This efficient formulation is arrived at by writing out the
* entire chain rule product dRdq * dqdv in terms of 'q' and
* noticing that all the entries are formed from sums of just
* nine products of 'q' and 'dqdv' */
prod[0] = -MT_Scalar(4)*m_q.x()*dQdvi.x();
prod[1] = -MT_Scalar(4)*m_q.y()*dQdvi.y();
prod[2] = -MT_Scalar(4)*m_q.z()*dQdvi.z();
prod[3] = MT_Scalar(2)*(m_q.y()*dQdvi.x() + m_q.x()*dQdvi.y());
prod[4] = MT_Scalar(2)*(m_q.w()*dQdvi.z() + m_q.z()*dQdvi.w());
prod[5] = MT_Scalar(2)*(m_q.z()*dQdvi.x() + m_q.x()*dQdvi.z());
prod[6] = MT_Scalar(2)*(m_q.w()*dQdvi.y() + m_q.y()*dQdvi.w());
prod[7] = MT_Scalar(2)*(m_q.z()*dQdvi.y() + m_q.y()*dQdvi.z());
prod[8] = MT_Scalar(2)*(m_q.w()*dQdvi.x() + m_q.x()*dQdvi.w());
/* first row, followed by second and third */
dRdvi[0][0] = prod[1] + prod[2];
dRdvi[0][1] = prod[3] - prod[4];
dRdvi[0][2] = prod[5] + prod[6];
dRdvi[1][0] = prod[3] + prod[4];
dRdvi[1][1] = prod[0] + prod[2];
dRdvi[1][2] = prod[7] - prod[8];
dRdvi[2][0] = prod[5] - prod[6];
dRdvi[2][1] = prod[7] + prod[8];
dRdvi[2][2] = prod[0] + prod[1];
}
void MT_Transform::setIdentity() {
m_basis.setIdentity();
m_origin.setValue(MT_Scalar(0.0f), MT_Scalar(0.0f), MT_Scalar(0.0f));
m_type = IDENTITY;
}
void KX_Camera::ExtractFrustumSphere()
{
if (m_set_frustum_center)
return;
// compute sphere for the general case and not only symmetric frustum:
// the mirror code in ImageRender can use very asymmetric frustum.
// We will put the sphere center on the line that goes from origin to the center of the far clipping plane
// This is the optimal position if the frustum is symmetric or very asymmetric and probably close
// to optimal for the general case. The sphere center position is computed so that the distance to
// the near and far extreme frustum points are equal.
// get the transformation matrix from device coordinate to camera coordinate
MT_Matrix4x4 clip_camcs_matrix = m_projection_matrix;
clip_camcs_matrix.invert();
if (m_projection_matrix[3][3] == MT_Scalar(0.0))
{
// frustrum projection
// detect which of the corner of the far clipping plane is the farthest to the origin
MT_Vector4 nfar; // far point in device normalized coordinate
MT_Point3 farpoint; // most extreme far point in camera coordinate
MT_Point3 nearpoint;// most extreme near point in camera coordinate
MT_Point3 farcenter(0.0, 0.0, 0.0);// center of far cliping plane in camera coordinate
MT_Scalar F=-1.0, N; // square distance of far and near point to origin
MT_Scalar f, n; // distance of far and near point to z axis. f is always > 0 but n can be < 0
MT_Scalar e, s; // far and near clipping distance (<0)
MT_Scalar c; // slope of center line = distance of far clipping center to z axis / far clipping distance
MT_Scalar z; // projection of sphere center on z axis (<0)
// tmp value
MT_Vector4 npoint(1.0, 1.0, 1.0, 1.0);
MT_Vector4 hpoint;
MT_Point3 point;
MT_Scalar len;
for (int i=0; i<4; i++)
{
hpoint = clip_camcs_matrix*npoint;
point.setValue(hpoint[0]/hpoint[3], hpoint[1]/hpoint[3], hpoint[2]/hpoint[3]);
len = point.dot(point);
if (len > F)
{
nfar = npoint;
farpoint = point;
F = len;
}
// rotate by 90 degree along the z axis to walk through the 4 extreme points of the far clipping plane
len = npoint[0];
npoint[0] = -npoint[1];
npoint[1] = len;
farcenter += point;
}
// the far center is the average of the far clipping points
farcenter *= 0.25;
// the extreme near point is the opposite point on the near clipping plane
nfar.setValue(-nfar[0], -nfar[1], -1.0, 1.0);
nfar = clip_camcs_matrix*nfar;
nearpoint.setValue(nfar[0]/nfar[3], nfar[1]/nfar[3], nfar[2]/nfar[3]);
// this is a frustrum projection
N = nearpoint.dot(nearpoint);
e = farpoint[2];
s = nearpoint[2];
// projection on XY plane for distance to axis computation
MT_Point2 farxy(farpoint[0], farpoint[1]);
// f is forced positive by construction
f = farxy.length();
// get corresponding point on the near plane
farxy *= s/e;
// this formula preserve the sign of n
n = f*s/e - MT_Point2(nearpoint[0]-farxy[0], nearpoint[1]-farxy[1]).length();
c = MT_Point2(farcenter[0], farcenter[1]).length()/e;
// the big formula, it simplifies to (F-N)/(2(e-s)) for the symmetric case
z = (F-N)/(2.0*(e-s+c*(f-n)));
m_frustum_center = MT_Point3(farcenter[0]*z/e, farcenter[1]*z/e, z);
m_frustum_radius = m_frustum_center.distance(farpoint);
}
else
{
// orthographic projection
// The most extreme points on the near and far plane. (normalized device coords)
MT_Vector4 hnear(1.0, 1.0, 1.0, 1.0), hfar(-1.0, -1.0, -1.0, 1.0);
// Transform to hom camera local space
hnear = clip_camcs_matrix*hnear;
hfar = clip_camcs_matrix*hfar;
// Tranform to 3d camera local space.
MT_Point3 nearpoint(hnear[0]/hnear[3], hnear[1]/hnear[3], hnear[2]/hnear[3]);
MT_Point3 farpoint(hfar[0]/hfar[3], hfar[1]/hfar[3], hfar[2]/hfar[3]);
// just use mediant point
m_frustum_center = (farpoint + nearpoint)*0.5;
m_frustum_radius = m_frustum_center.distance(farpoint);
}
// Transform to world space.
m_frustum_center = GetCameraToWorld()(m_frustum_center);
m_frustum_radius /= fabs(NodeGetWorldScaling()[NodeGetWorldScaling().closestAxis()]);
m_set_frustum_center = true;
}
MT_Scalar KX_BulletPhysicsController::GetRadius()
{
return MT_Scalar(CcdPhysicsController::GetRadius());
}
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
}
void ImageRender::Render()
{
RAS_FrameFrustum frustrum;
if (!m_render)
return;
if (m_mirror)
{
// mirror mode, compute camera frustrum, position and orientation
// convert mirror position and normal in world space
const MT_Matrix3x3 & mirrorObjWorldOri = m_mirror->GetSGNode()->GetWorldOrientation();
const MT_Point3 & mirrorObjWorldPos = m_mirror->GetSGNode()->GetWorldPosition();
const MT_Vector3 & mirrorObjWorldScale = m_mirror->GetSGNode()->GetWorldScaling();
MT_Point3 mirrorWorldPos =
mirrorObjWorldPos + mirrorObjWorldScale * (mirrorObjWorldOri * m_mirrorPos);
MT_Vector3 mirrorWorldZ = mirrorObjWorldOri * m_mirrorZ;
// get observer world position
const MT_Point3 & observerWorldPos = m_observer->GetSGNode()->GetWorldPosition();
// get plane D term = mirrorPos . normal
MT_Scalar mirrorPlaneDTerm = mirrorWorldPos.dot(mirrorWorldZ);
// compute distance of observer to mirror = D - observerPos . normal
MT_Scalar observerDistance = mirrorPlaneDTerm - observerWorldPos.dot(mirrorWorldZ);
// if distance < 0.01 => observer is on wrong side of mirror, don't render
if (observerDistance < 0.01)
return;
// set camera world position = observerPos + normal * 2 * distance
MT_Point3 cameraWorldPos = observerWorldPos + (MT_Scalar(2.0)*observerDistance)*mirrorWorldZ;
m_camera->GetSGNode()->SetLocalPosition(cameraWorldPos);
// set camera orientation: z=normal, y=mirror_up in world space, x= y x z
MT_Vector3 mirrorWorldY = mirrorObjWorldOri * m_mirrorY;
MT_Vector3 mirrorWorldX = mirrorObjWorldOri * m_mirrorX;
MT_Matrix3x3 cameraWorldOri(
mirrorWorldX[0], mirrorWorldY[0], mirrorWorldZ[0],
mirrorWorldX[1], mirrorWorldY[1], mirrorWorldZ[1],
mirrorWorldX[2], mirrorWorldY[2], mirrorWorldZ[2]);
m_camera->GetSGNode()->SetLocalOrientation(cameraWorldOri);
m_camera->GetSGNode()->UpdateWorldData(0.0);
// compute camera frustrum:
// get position of mirror relative to camera: offset = mirrorPos-cameraPos
MT_Vector3 mirrorOffset = mirrorWorldPos - cameraWorldPos;
// convert to camera orientation
mirrorOffset = mirrorOffset * cameraWorldOri;
// scale mirror size to world scale:
// get closest local axis for mirror Y and X axis and scale height and width by local axis scale
MT_Scalar x, y;
x = fabs(m_mirrorY[0]);
y = fabs(m_mirrorY[1]);
float height = (x > y) ?
((x > fabs(m_mirrorY[2])) ? mirrorObjWorldScale[0] : mirrorObjWorldScale[2]):
((y > fabs(m_mirrorY[2])) ? mirrorObjWorldScale[1] : mirrorObjWorldScale[2]);
x = fabs(m_mirrorX[0]);
y = fabs(m_mirrorX[1]);
float width = (x > y) ?
((x > fabs(m_mirrorX[2])) ? mirrorObjWorldScale[0] : mirrorObjWorldScale[2]):
((y > fabs(m_mirrorX[2])) ? mirrorObjWorldScale[1] : mirrorObjWorldScale[2]);
width *= m_mirrorHalfWidth;
height *= m_mirrorHalfHeight;
// left = offsetx-width
// right = offsetx+width
// top = offsety+height
// bottom = offsety-height
// near = -offsetz
// far = near+100
frustrum.x1 = mirrorOffset[0]-width;
frustrum.x2 = mirrorOffset[0]+width;
frustrum.y1 = mirrorOffset[1]-height;
frustrum.y2 = mirrorOffset[1]+height;
frustrum.camnear = -mirrorOffset[2];
frustrum.camfar = -mirrorOffset[2]+m_clip;
}
// Store settings to be restored later
const RAS_IRasterizer::StereoMode stereomode = m_rasterizer->GetStereoMode();
RAS_Rect area = m_canvas->GetWindowArea();
// The screen area that ImageViewport will copy is also the rendering zone
m_canvas->SetViewPort(m_position[0], m_position[1], m_position[0]+m_capSize[0]-1, m_position[1]+m_capSize[1]-1);
m_canvas->ClearColor(m_background[0], m_background[1], m_background[2], m_background[3]);
m_canvas->ClearBuffer(RAS_ICanvas::COLOR_BUFFER|RAS_ICanvas::DEPTH_BUFFER);
m_rasterizer->BeginFrame(m_engine->GetClockTime());
m_scene->GetWorldInfo()->UpdateWorldSettings();
m_rasterizer->SetAuxilaryClientInfo(m_scene);
m_rasterizer->DisplayFog();
// matrix calculation, don't apply any of the stereo mode
m_rasterizer->SetStereoMode(RAS_IRasterizer::RAS_STEREO_NOSTEREO);
if (m_mirror)
{
// frustrum was computed above
// get frustrum matrix and set projection matrix
MT_Matrix4x4 projmat = m_rasterizer->GetFrustumMatrix(
frustrum.x1, frustrum.x2, frustrum.y1, frustrum.y2, frustrum.camnear, frustrum.camfar);
m_camera->SetProjectionMatrix(projmat);
}
else if (m_camera->hasValidProjectionMatrix()) {
m_rasterizer->SetProjectionMatrix(m_camera->GetProjectionMatrix());
}
else {
float lens = m_camera->GetLens();
float sensor_x = m_camera->GetSensorWidth();
float sensor_y = m_camera->GetSensorHeight();
float shift_x = m_camera->GetShiftHorizontal();
float shift_y = m_camera->GetShiftVertical();
bool orthographic = !m_camera->GetCameraData()->m_perspective;
float nearfrust = m_camera->GetCameraNear();
float farfrust = m_camera->GetCameraFar();
float aspect_ratio = 1.0f;
Scene *blenderScene = m_scene->GetBlenderScene();
MT_Matrix4x4 projmat;
// compute the aspect ratio from frame blender scene settings so that render to texture
// works the same in Blender and in Blender player
if (blenderScene->r.ysch != 0)
aspect_ratio = float(blenderScene->r.xsch*blenderScene->r.xasp) / float(blenderScene->r.ysch*blenderScene->r.yasp);
if (orthographic) {
RAS_FramingManager::ComputeDefaultOrtho(
nearfrust,
farfrust,
m_camera->GetScale(),
aspect_ratio,
m_camera->GetSensorFit(),
shift_x,
shift_y,
frustrum
);
projmat = m_rasterizer->GetOrthoMatrix(
frustrum.x1, frustrum.x2, frustrum.y1, frustrum.y2, frustrum.camnear, frustrum.camfar);
}
else {
RAS_FramingManager::ComputeDefaultFrustum(
nearfrust,
farfrust,
lens,
sensor_x,
sensor_y,
RAS_SENSORFIT_AUTO,
shift_x,
shift_y,
aspect_ratio,
frustrum);
projmat = m_rasterizer->GetFrustumMatrix(
frustrum.x1, frustrum.x2, frustrum.y1, frustrum.y2, frustrum.camnear, frustrum.camfar);
}
m_camera->SetProjectionMatrix(projmat);
}
MT_Transform camtrans(m_camera->GetWorldToCamera());
MT_Matrix4x4 viewmat(camtrans);
m_rasterizer->SetViewMatrix(viewmat, m_camera->NodeGetWorldOrientation(), m_camera->NodeGetWorldPosition(), m_camera->GetCameraData()->m_perspective);
m_camera->SetModelviewMatrix(viewmat);
// restore the stereo mode now that the matrix is computed
m_rasterizer->SetStereoMode(stereomode);
if (stereomode == RAS_IRasterizer::RAS_STEREO_QUADBUFFERED) {
// In QUAD buffer stereo mode, the GE render pass ends with the right eye on the right buffer
// but we need to draw on the left buffer to capture the render
// TODO: implement an explicit function in rasterizer to restore the left buffer.
m_rasterizer->SetEye(RAS_IRasterizer::RAS_STEREO_LEFTEYE);
}
m_scene->CalculateVisibleMeshes(m_rasterizer,m_camera);
m_engine->UpdateAnimations(m_scene);
m_scene->RenderBuckets(camtrans, m_rasterizer);
m_scene->RenderFonts();
// restore the canvas area now that the render is completed
m_canvas->GetWindowArea() = area;
}
bool
KX_SlowParentRelation::
UpdateChildCoordinates(
SG_Spatial * child,
const SG_Spatial * parent,
bool& parentUpdated
) {
MT_assert(child != NULL);
// the child will move even if the parent is not
parentUpdated = true;
const MT_Vector3 & child_scale = child->GetLocalScale();
const MT_Point3 & child_pos = child->GetLocalPosition();
const MT_Matrix3x3 & child_rotation = child->GetLocalOrientation();
// the childs world locations which we will update.
MT_Vector3 child_w_scale;
MT_Point3 child_w_pos;
MT_Matrix3x3 child_w_rotation;
if (parent) {
// This is a slow parent relation
// first compute the normal child world coordinates.
MT_Vector3 child_n_scale;
MT_Point3 child_n_pos;
MT_Matrix3x3 child_n_rotation;
const MT_Vector3 & p_world_scale = parent->GetWorldScaling();
const MT_Point3 & p_world_pos = parent->GetWorldPosition();
const MT_Matrix3x3 & p_world_rotation = parent->GetWorldOrientation();
child_n_scale = p_world_scale * child_scale;
child_n_rotation = p_world_rotation * child_rotation;
child_n_pos = p_world_pos + p_world_scale *
(p_world_rotation * child_pos);
if (m_initialized) {
// get the current world positions
child_w_scale = child->GetWorldScaling();
child_w_pos = child->GetWorldPosition();
child_w_rotation = child->GetWorldOrientation();
// now 'interpolate' the normal coordinates with the last
// world coordinates to get the new world coordinates.
MT_Scalar weight = MT_Scalar(1)/(m_relax + 1);
child_w_scale = (m_relax * child_w_scale + child_n_scale) * weight;
child_w_pos = (m_relax * child_w_pos + child_n_pos) * weight;
// for rotation we must go through quaternion
MT_Quaternion child_w_quat = child_w_rotation.getRotation().slerp(child_n_rotation.getRotation(), weight);
child_w_rotation.setRotation(child_w_quat);
//FIXME: update physics controller.
} else {
child_w_scale = child_n_scale;
child_w_pos = child_n_pos;
child_w_rotation = child_n_rotation;
m_initialized = true;
}
} else {
child_w_scale = child_scale;
child_w_pos = child_pos;
child_w_rotation = child_rotation;
}
child->SetWorldScale(child_w_scale);
child->SetWorldPosition(child_w_pos);
child->SetWorldOrientation(child_w_rotation);
child->ClearModified();
// this node must always be updated, so reschedule it for next time
child->ActivateRecheduleUpdateCallback();
return true; //parent != NULL;
}
