scalar vpSystem::GetPotentialEnergy(void) const
{
int i;
scalar PotentialEnergy = SCALAR_0;
for ( i = 0; i < m_pJoint.size(); i++ )
PotentialEnergy += m_pJoint[i]->GetPotentialEnergy();
for ( i = 0; i < m_pBody.size(); i++ )
if ( !m_pBody[i]->IsGround() ) PotentialEnergy -= m_pBody[i]->m_sI.GetMass() * Inner(m_pBody[i]->m_sFrame * m_pBody[i]->m_sCenterOfMass, m_pWorld->m_sGravity);
return PotentialEnergy;
}
void dLineClosestApproach (const dVector3 pa, const dVector3 ua,
const dVector3 pb, const dVector3 ub,
double *alpha, double *beta)
{
dVector3 p;
p[0] = pb[0] - pa[0];
p[1] = pb[1] - pa[1];
p[2] = pb[2] - pa[2];
double uaub = Inner(ua,ub);
double q1 = Inner(ua,p);
double q2 = -Inner(ub,p);
double d = 1-uaub*uaub;
if (d <= 0) {
// @@@ this needs to be made more robust
*alpha = 0;
*beta = 0;
}
else {
d = dRecip(d);
*alpha = (q1 + uaub*q2)*d;
*beta = (uaub*q1 + q2)*d;
}
}
inline bool is_numerical( boost::optional<Inner> ) { return is_numerical(Inner()); }
Inner Outer::MyMethod(Inner arg) { // expected-error {{unknown type name 'Inner'; did you mean 'Outer::Inner'?}}
return Inner();
}
// given two boxes (p1,R1,side1) and (p2,R2,side2), collide them together and
// generate contact points. this returns 0 if there is no contact otherwise
// it returns the number of contacts generated.
// `normal' returns the contact normal.
// `depth' returns the maximum penetration depth along that normal.
// `return_code' returns a number indicating the type of contact that was
// detected:
// 1,2,3 = box 2 intersects with a face of box 1
// 4,5,6 = box 1 intersects with a face of box 2
// 7..15 = edge-edge contact
// `maxc' is the maximum number of contacts allowed to be generated, i.e.
// the size of the `contact' array.
// `contact' and `skip' are the contact array information provided to the
// collision functions. this function only fills in the position and depth
// fields.
int dBoxBox(const dVector3 p1, const dMatrix3 R1, const dVector3 side1,
const dVector3 p2, const dMatrix3 R2, const dVector3 side2,
std::vector<Contact>& result)
{
const double fudge_factor = 1.05;
dVector3 p,pp,normalC = {0.0, 0.0, 0.0, 0.0};
const double *normalR = 0;
double A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33,Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l;
int i,j,invert_normal,code;
// get vector from centers of box 1 to box 2, relative to box 1
p[0] = p2[0] - p1[0];
p[1] = p2[1] - p1[1];
p[2] = p2[2] - p1[2];
dMULTIPLY1_331 (pp,R1,p); // get pp = p relative to body 1
// get side lengths / 2
A[0] = side1[0];
A[1] = side1[1];
A[2] = side1[2];
B[0] = side2[0];
B[1] = side2[1];
B[2] = side2[2];
// Rij is R1'*R2, i.e. the relative rotation between R1 and R2
R11 = Inner44(R1+0,R2+0); R12 = Inner44(R1+0,R2+1); R13 = Inner44(R1+0,R2+2);
R21 = Inner44(R1+1,R2+0); R22 = Inner44(R1+1,R2+1); R23 = Inner44(R1+1,R2+2);
R31 = Inner44(R1+2,R2+0); R32 = Inner44(R1+2,R2+1); R33 = Inner44(R1+2,R2+2);
Q11 = std::abs(R11); Q12 = std::abs(R12); Q13 = std::abs(R13);
Q21 = std::abs(R21); Q22 = std::abs(R22); Q23 = std::abs(R23);
Q31 = std::abs(R31); Q32 = std::abs(R32); Q33 = std::abs(R33);
// for all 15 possible separating axes:
// * see if the axis separates the boxes. if so, return 0.
// * find the depth of the penetration along the separating axis (s2)
// * if this is the largest depth so far, record it.
// the normal vector will be set to the separating axis with the smallest
// depth. note: normalR is set to point to a column of R1 or R2 if that is
// the smallest depth normal so far. otherwise normalR is 0 and normalC is
// set to a vector relative to body 1. invert_normal is 1 if the sign of
// the normal should be flipped.
#define TST(expr1,expr2,norm,cc) \
s2 = std::abs(expr1) - (expr2); \
if (s2 > s) { \
s = s2; \
normalR = norm; \
invert_normal = ((expr1) < 0); \
code = (cc); \
}
s = -1E12;
invert_normal = 0;
code = 0;
// separating axis = u1,u2,u3
TST (pp[0],(A[0] + B[0]*Q11 + B[1]*Q12 + B[2]*Q13),R1+0,1);
TST (pp[1],(A[1] + B[0]*Q21 + B[1]*Q22 + B[2]*Q23),R1+1,2);
TST (pp[2],(A[2] + B[0]*Q31 + B[1]*Q32 + B[2]*Q33),R1+2,3);
// separating axis = v1,v2,v3
TST (Inner41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4);
TST (Inner41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5);
TST (Inner41(R2+2,p),(A[0]*Q13 + A[1]*Q23 + A[2]*Q33 + B[2]),R2+2,6);
// note: cross product axes need to be scaled when s is computed.
// normal (n1,n2,n3) is relative to box 1.
#undef TST
#define TST(expr1,expr2,n1,n2,n3,cc) \
s2 = std::abs(expr1) - (expr2); \
l = sqrt ((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \
if (l > 0) { \
s2 /= l; \
if (s2*fudge_factor > s) { \
s = s2; \
normalR = 0; \
normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \
invert_normal = ((expr1) < 0); \
code = (cc); \
} \
}
// separating axis = u1 x (v1,v2,v3)
TST(pp[2]*R21-pp[1]*R31,(A[1]*Q31+A[2]*Q21+B[1]*Q13+B[2]*Q12),0,-R31,R21,7);
TST(pp[2]*R22-pp[1]*R32,(A[1]*Q32+A[2]*Q22+B[0]*Q13+B[2]*Q11),0,-R32,R22,8);
TST(pp[2]*R23-pp[1]*R33,(A[1]*Q33+A[2]*Q23+B[0]*Q12+B[1]*Q11),0,-R33,R23,9);
// separating axis = u2 x (v1,v2,v3)
TST(pp[0]*R31-pp[2]*R11,(A[0]*Q31+A[2]*Q11+B[1]*Q23+B[2]*Q22),R31,0,-R11,10);
TST(pp[0]*R32-pp[2]*R12,(A[0]*Q32+A[2]*Q12+B[0]*Q23+B[2]*Q21),R32,0,-R12,11);
TST(pp[0]*R33-pp[2]*R13,(A[0]*Q33+A[2]*Q13+B[0]*Q22+B[1]*Q21),R33,0,-R13,12);
// separating axis = u3 x (v1,v2,v3)
TST(pp[1]*R11-pp[0]*R21,(A[0]*Q21+A[1]*Q11+B[1]*Q33+B[2]*Q32),-R21,R11,0,13);
TST(pp[1]*R12-pp[0]*R22,(A[0]*Q22+A[1]*Q12+B[0]*Q33+B[2]*Q31),-R22,R12,0,14);
TST(pp[1]*R13-pp[0]*R23,(A[0]*Q23+A[1]*Q13+B[0]*Q32+B[1]*Q31),-R23,R13,0,15);
#undef TST
if (!code) return 0;
if (s > 0.0) return 0;
// if we get to this point, the boxes interpenetrate. compute the normal
// in global coordinates.
Eigen::Vector3d normal;
Eigen::Vector3d point_vec;
double penetration;
if (normalR) {
normal << normalR[0],normalR[4],normalR[8];
}
else {
normal << Inner((R1),(normalC)), Inner((R1+4),(normalC)), Inner((R1+8),(normalC));
//dMULTIPLY0_331 (normal,R1,normalC);
}
if (invert_normal) {
normal *= -1.0;
}
// compute contact point(s)
// single point
if (code > 6) {
// an edge from box 1 touches an edge from box 2.
// find a point pa on the intersecting edge of box 1
dVector3 pa;
double sign;
for (i=0; i<3; i++) pa[i] = p1[i];
for (j=0; j<3; j++)
{
#define TEMP_INNER14(a,b) (a[0]*(b)[0] + a[1]*(b)[4] + a[2]*(b)[8])
sign = (TEMP_INNER14(normal,R1+j) > 0) ? 1.0 : -1.0;
//sign = (Inner14(normal,R1+j) > 0) ? 1.0 : -1.0;
for (i=0; i<3; i++) pa[i] += sign * A[j] * R1[i*4+j];
}
// find a point pb on the intersecting edge of box 2
dVector3 pb;
for (i=0; i<3; i++) pb[i] = p2[i];
for (j=0; j<3; j++) {
sign = (TEMP_INNER14(normal,R2+j) > 0) ? -1.0 : 1.0;
#undef TEMP_INNER14
for (i=0; i<3; i++) pb[i] += sign * B[j] * R2[i*4+j];
}
double alpha,beta;
dVector3 ua,ub;
for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4];
for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4];
dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta);
for (i=0; i<3; i++) pa[i] += ua[i]*alpha;
for (i=0; i<3; i++) pb[i] += ub[i]*beta;
{
point_vec << 0.5*(pa[0]+pb[0]), 0.5*(pa[1]+pb[1]), 0.5*(pa[2]+pb[2]);
penetration = -s;
Contact contact;
contact.point = point_vec;
contact.normal = normal;
contact.penetrationDepth = penetration;
result.push_back(contact);
}
return 1;
}
// okay, we have a face-something intersection (because the separating
// axis is perpendicular to a face). define face 'a' to be the reference
// face (i.e. the normal vector is perpendicular to this) and face 'b' to be
// the incident face (the closest face of the other box).
const double *Ra,*Rb,*pa,*pb,*Sa,*Sb;
if (code <= 3) {
Ra = R1;
Rb = R2;
pa = p1;
pb = p2;
Sa = A;
Sb = B;
}
else {
Ra = R2;
Rb = R1;
pa = p2;
pb = p1;
Sa = B;
Sb = A;
}
// nr = normal vector of reference face dotted with axes of incident box.
// anr = absolute values of nr.
dVector3 normal2,nr,anr;
if (code <= 3) {
normal2[0] = normal[0];
normal2[1] = normal[1];
normal2[2] = normal[2];
}
else {
normal2[0] = -normal[0];
normal2[1] = -normal[1];
normal2[2] = -normal[2];
}
dMULTIPLY1_331 (nr,Rb,normal2);
anr[0] = fabs (nr[0]);
anr[1] = fabs (nr[1]);
anr[2] = fabs (nr[2]);
// find the largest compontent of anr: this corresponds to the normal
// for the indident face. the other axis numbers of the indicent face
// are stored in a1,a2.
int lanr,a1,a2;
if (anr[1] > anr[0]) {
if (anr[1] > anr[2]) {
a1 = 0;
lanr = 1;
a2 = 2;
}
else {
a1 = 0;
a2 = 1;
lanr = 2;
}
}
else {
if (anr[0] > anr[2]) {
lanr = 0;
a1 = 1;
a2 = 2;
}
else {
a1 = 0;
a2 = 1;
lanr = 2;
}
}
// compute center point of incident face, in reference-face coordinates
dVector3 center;
if (nr[lanr] < 0) {
for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i*4+lanr];
}
else {
for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i*4+lanr];
}
// find the normal and non-normal axis numbers of the reference box
int codeN,code1,code2;
if (code <= 3) codeN = code-1; else codeN = code-4;
if (codeN==0) {
code1 = 1;
code2 = 2;
}
else if (codeN==1) {
code1 = 0;
code2 = 2;
}
else {
code1 = 0;
code2 = 1;
}
// find the four corners of the incident face, in reference-face coordinates
double quad[8]; // 2D coordinate of incident face (x,y pairs)
double c1,c2,m11,m12,m21,m22;
c1 = Inner14 (center,Ra+code1);
c2 = Inner14 (center,Ra+code2);
// optimize this? - we have already computed this data above, but it is not
// stored in an easy-to-index format. for now it's quicker just to recompute
// the four dot products.
m11 = Inner44 (Ra+code1,Rb+a1);
m12 = Inner44 (Ra+code1,Rb+a2);
m21 = Inner44 (Ra+code2,Rb+a1);
m22 = Inner44 (Ra+code2,Rb+a2);
{
double k1 = m11*Sb[a1];
double k2 = m21*Sb[a1];
double k3 = m12*Sb[a2];
double k4 = m22*Sb[a2];
quad[0] = c1 - k1 - k3;
quad[1] = c2 - k2 - k4;
quad[2] = c1 - k1 + k3;
quad[3] = c2 - k2 + k4;
quad[4] = c1 + k1 + k3;
quad[5] = c2 + k2 + k4;
quad[6] = c1 + k1 - k3;
quad[7] = c2 + k2 - k4;
}
// find the size of the reference face
double rect[2];
rect[0] = Sa[code1];
rect[1] = Sa[code2];
// intersect the incident and reference faces
double ret[16];
int n = intersectRectQuad (rect,quad,ret);
if (n < 1) return 0; // this should never happen
// convert the intersection points into reference-face coordinates,
// and compute the contact position and depth for each point. only keep
// those points that have a positive (penetrating) depth. delete points in
// the 'ret' array as necessary so that 'point' and 'ret' correspond.
//real point[3*8]; // penetrating contact points
double point[24]; // penetrating contact points
double dep[8]; // depths for those points
double det1 = dRecip(m11*m22 - m12*m21);
m11 *= det1;
m12 *= det1;
m21 *= det1;
m22 *= det1;
int cnum = 0; // number of penetrating contact points found
for (j=0; j < n; j++) {
double k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2);
double k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2);
for (i=0; i<3; i++)
{
point[cnum*3+i] = center[i] + k1*Rb[i*4+a1] + k2*Rb[i*4+a2];
}
dep[cnum] = Sa[codeN] - Inner(normal2,point+cnum*3);
if (dep[cnum] >= 0) {
ret[cnum*2] = ret[j*2];
ret[cnum*2+1] = ret[j*2+1];
cnum++;
}
}
if (cnum < 1) return 0; // this should never happen
// we can't generate more contacts than we actually have
int maxc = 4;
if (maxc > cnum) maxc = cnum;
//if (maxc < 1) maxc = 1;
if (cnum <= maxc) {
// we have less contacts than we need, so we use them all
for (j=0; j < cnum; j++)
{
point_vec << point[j*3+0] + pa[0], point[j*3+1] + pa[1], point[j*3+2] + pa[2];
Contact contact;
contact.point = point_vec;
contact.normal = normal;
contact.penetrationDepth = dep[j];
result.push_back(contact);
}
}
else {
// we have more contacts than are wanted, some of them must be culled.
// find the deepest point, it is always the first contact.
int i1 = 0;
double maxdepth = dep[0];
for (i=1; i<cnum; i++) {
if (dep[i] > maxdepth) {
maxdepth = dep[i];
i1 = i;
}
}
int iret[8];
cullPoints (cnum,ret,maxc,i1,iret);
cnum = maxc;
for (j=0; j < cnum; j++)
{
point_vec << point[iret[j]*3+0] + pa[0], point[iret[j]*3+1] + pa[1], point[iret[j]*3+2] + pa[2];
Contact contact;
contact.point = point_vec;
contact.normal = normal;
contact.penetrationDepth = dep[iret[j]];
result.push_back(contact);
}
}
return cnum;
}
void add_attributes( boost::optional<Inner>, boost::ptr_vector< BaseAttribute >& t ) {
add_attributes( Inner(), t );
t.push_back( new Attribute<bool>("is_optional", true) );
}
void Entity::Move(ID3D11Device* device, Vector3_t moveAccel, GameWorld* gameWorld, float frameTime)
{
// CHANGE MOVE_BITMASK TO V3 inAccel
float dt = frameTime/1000.0f;
Vector3_t accelVec = {0, 0, 0};
// Get the rotation in radians to correct in the movement
float moveDirCos = 1.0f;
float strafeDirCos = 0.0f;
// Set the acceleration vector to just the x & z components first for normalization purposes
accelVec.x = moveAccel.x;
accelVec.z = moveAccel.z;
// Normalize the X & Z movement
float accelLength = LengthSq(accelVec);
if(accelLength > 1.0f)
{
accelVec *= (1.0f / SquareRoot(accelLength));
}
// Twiddle factor for movment speed (8.0 is used to combat the drag for movement)
float entitySpeed = WALK_SPEED*8.0f;
accelVec *= entitySpeed;
// TODO(ebd): ODE here!
if (m_groundDragEn)
{
accelVec.x += -8.0f*m_velocity.x;
accelVec.z += -8.0f*m_velocity.z;
}
else
{
accelVec.x += -5.0f*m_velocity.x;
accelVec.z += -5.0f*m_velocity.z;
}
// Now add in the y acceleration value
accelVec.y = moveAccel.y*entitySpeed*20.0f;
// Add gravity
accelVec.y -= GRAVITY_ACCEL;
// Now do physics based movement
// Need to add Physics objects to the entity
// RigidBody object -> holds postion, rotation, velocity, accel, and handles movement for the entity
// Collider object -> holds information about the bounding box and handles collision functions
// m_RigidBody->Move(&oldPosition);
// m_Collider->CollisionTest(&position, &rotation); // This should return a bool for if there was a collision or not
// Do the movement internally for now
Vector3_t oldPosition = m_position;
Vector3_t positionDelta = (0.5f*accelVec*Square(dt) + m_velocity*dt);
m_velocity = accelVec*dt + m_velocity;
m_position = oldPosition + positionDelta;
// Do minkowski based collision here
// First get a list of game map tiles to search through
/*
uint32 minTileX = FloorFloattoUint32(Minimum(m_position.x, oldPosition.x) - m_aabb.x/2);
uint32 maxTileX = FloorFloattoUint32(Maximum(m_position.x, oldPosition.x) + m_aabb.x/2);
uint32 minTileZ = FloorFloattoUint32(Minimum(m_position.z, oldPosition.z) - m_aabb.z/2);
uint32 maxTileZ = FloorFloattoUint32(Maximum(m_position.z, oldPosition.z) + m_aabb.z/2);
*/
uint32 minTileX = FloorFloattoUint32(Minimum(m_position.x, oldPosition.x));
uint32 maxTileX = FloorFloattoUint32(Maximum(m_position.x, oldPosition.x));
uint32 minTileZ = FloorFloattoUint32(Minimum(m_position.z, oldPosition.z));
uint32 maxTileZ = FloorFloattoUint32(Maximum(m_position.z, oldPosition.z));
// Set the time remaining on the search to 1.0
// We use this to find where the collision was along the movement vector
float tMin = 1.0f;
Vector3_t collisionNormal = {0.0f, 0.0f, 0.0f};
bool floorCollision = false;
// Test each tile in the search space
for (uint32 tileX = minTileX; tileX <= maxTileX; tileX++)
{
for (uint32 tileZ = minTileZ; tileZ <= maxTileZ; tileZ++)
{
// Get the normal for the current tile (is it a wall or floor or neither)
collisionNormal = gameWorld->GetTileNormal(tileX, tileZ);
// Get the current tile value, and if there is a valid tile then do collision there
if (collisionNormal.y == 1.0f)
{
if (TestFloorCollision(oldPosition, positionDelta, &tMin))
{
collisionNormal = {0.0f, 1.0f, 0.0f};
floorCollision = true;
}
}
}
}
if (!floorCollision)
collisionNormal = {0.0f, 0.0f, 0.0f};
// Now update the entity position based on the output of the collision detection
// Update the timeRemaining using tMin
m_position.y = oldPosition.y + tMin*positionDelta.y;
m_velocity = m_velocity - 1*Inner(m_velocity, collisionNormal)*collisionNormal;
/*
if (m_position.y < 0)
{
m_position.y = 0;
m_velocity.y = 0;
}
*/
// Now updat the model position to be where the entity is
m_Model->UpdatePosition(device, m_position);
}