static inline
float CalcArea4Points(const Vector3 &p0, const Vector3 &p1, const Vector3 &p2, const Vector3 &p3){
float areaSqrtA = lengthSqr(cross(p0 - p1, p2 - p3));
float areaSqrtB = lengthSqr(cross(p0 - p2, p1 - p3));
float areaSqrtC = lengthSqr(cross(p0 - p3, p1 - p2));
return MAX(MAX(areaSqrtA, areaSqrtB), areaSqrtC);
}
void epxApplyExternalForce(
EpxState &state,
const EpxRigidBody &body,
const EpxVector3 &externalForce,
const EpxVector3 &externalTorque,
EpxFloat timeStep)
{
if(state.m_motionType == EpxMotionTypeStatic) {
return;
}
EpxMatrix3 orientation(state.m_orientation);
EpxMatrix3 worldInertia = orientation * body.m_inertia * transpose(orientation);
EpxMatrix3 worldInertiaInv = orientation * inverse(body.m_inertia) * transpose(orientation);
EpxVector3 angularMomentum = worldInertia * state.m_angularVelocity;
state.m_linearVelocity += externalForce / body.m_mass * timeStep;
angularMomentum += externalTorque * timeStep;
state.m_angularVelocity = worldInertiaInv * angularMomentum;
EpxFloat linVelSqr = lengthSqr(state.m_linearVelocity);
if(linVelSqr > (EPX_MAX_LINEAR_VELOCITY*EPX_MAX_LINEAR_VELOCITY)) {
state.m_linearVelocity = (state.m_linearVelocity/sqrtf(linVelSqr)) * EPX_MAX_LINEAR_VELOCITY;
}
EpxFloat angVelSqr = lengthSqr(state.m_angularVelocity);
if(angVelSqr > (EPX_MAX_ANGULAR_VELOCITY*EPX_MAX_ANGULAR_VELOCITY)) {
state.m_angularVelocity = (state.m_angularVelocity/sqrtf(angVelSqr)) * EPX_MAX_ANGULAR_VELOCITY;
}
}
// intersect sphere
inline bool intersect_sphere(const ray3f& ray, float radius, float& t) {
auto a = lengthSqr(ray.d);
auto b = 2*dot(ray.d,ray.e);
auto c = lengthSqr(ray.e) - radius*radius;
auto d = b*b-4*a*c;
if(d < 0) return false;
float tm = (-b-sqrt(d)) / (2*a);
float tM = (-b+sqrt(d)) / (2*a);
bool hitm = (tm >= ray.tmin and tm <= ray.tmax);
bool hitM = (tM >= ray.tmin and tM <= ray.tmax);
if(!hitm and !hitM) return false;
t = hitm ? tm : tM;
return true;
}
//同一衝突点の探索
//同じ衝突点を見つけたらそのインデックスを返す、みつからなければ-1
int Contact::FindNearestContactPoint(const Vector3 &newPointA, const Vector3 &newPointB, const Vector3 &newNormal){
int nearestIdx = -1;
float minDiff = CONTACT_SAME_POINT;
for (unsigned i = 0; i < m_numContacts; i++){
float diffA = lengthSqr(m_contactPoints[i].pointA - newPointA);
float diffB = lengthSqr(m_contactPoints[i].pointB - newPointB);
if (diffA < minDiff && diffB < minDiff && dot(newNormal, m_contactPoints[i].normal) > 0.99f){
minDiff = MAX(diffA, diffB);
nearestIdx = i;
}
}
return nearestIdx;
}
void Circle::intersect(const Circle& other, he::PrimitiveList<vec2>& outIntersections) const
{
float d(length(m_Position - other.getPosition()));
if (d > m_Radius + other.m_Radius)
{
return;
}
else if (d < fabs(m_Radius - other.m_Radius))
{
return;
}
else
{
//line connecting 2 points =
const float& x1(m_Position.x),
x2(other.m_Position.x),
y1(m_Position.y),
y2(other.m_Position.y),
r1(m_Radius),
r2(other.m_Radius);
float d2(lengthSqr(other.m_Position - m_Position));
float xPart1( (x2 + x1) / 2.0f + ((x2 - x1) * (sqr(r1) - sqr(r2))) / (2.0f * d2));
float xPart2( ((y2 - y1) / (2.0f * d2)) * sqrtf( (sqr(r1 + r2) - d2) * (d2 - sqr(r2 - r1))));
float yPart1( (y2 + y1) / 2.0f + ((y2 - y1) * (sqr(r1) - sqr(r2))) / (2.0f * d2));
float yPart2( ((x2 - x1) / (2.0f * d2)) * sqrtf( (sqr(r1 + r2) - d2) * (d2 - sqr(r2 - r1))));
outIntersections.add( vec2(xPart1 + xPart2, yPart1 - yPart2) );
outIntersections.add( vec2(xPart1 - xPart2, yPart1 + yPart2) );
}
}
// 衝突点をリフレッシュ
void Contact::Refresh(const Vector3 &pA, const Quat &qA, const Vector3 &pB, const Quat &qB){
//衝突点更新
//衝突点間の距離が閾値を超えたら消去
for (int i = 0; i < (int)m_numContacts; i++){
Vector3 normal = m_contactPoints[i].normal;
Vector3 cpA = pA + rotate(qA, m_contactPoints[i].pointA);
Vector3 cpB = pB + rotate(qB, m_contactPoints[i].pointB);
// 貫通深度がプラスに転じたかどうかをチェック
float distance = dot(normal, cpA - cpB);
if (distance > CONTACT_THRESHOLD_NORMAL) {
RemoveContactPoint(i);
i--;
continue;
}
m_contactPoints[i].distance = distance;
// 深度方向を除去して両点の距離をチェック
cpA = cpA - m_contactPoints[i].distance * normal;
float distanceAB = lengthSqr(cpA - cpB);
if (distanceAB > CONTACT_THRESHOLD_TANGENT) {
RemoveContactPoint(i);
i--;
continue;
}
}
}
bool Sphere::isOtherInside(const Sphere& other) const
{
vec3 axis(other.m_Position - m_Position);
if (lengthSqr(axis) < sqr(m_Radius - other.getRadius()))
return true;
return false;
}
void RomShip::Tick(float DeltaTime)
{
Velocity += Acceleration * DeltaTime;
if(lengthSqr(Acceleration) < 0.01f)
Velocity = Vector3(0.0f, 0.0f, 0.0f);
Location += Velocity * DeltaTime;
Rotation *= DeltaRot;
DeltaRot = Quat::identity();
}
//-------------------------------------------------------
void voxTorusShape::getNormal (const coAABB& _aabb, coVec3& _normal) const
{
const coVec3 center = _aabb.getCenter();
const coFloat tmp = 4.f * (lengthSqr(center) - coMath_f::pow2(m_radii[0]) - coMath_f::pow2(m_radii[1]));
_normal[0] = center[0] * tmp;
_normal[1] = center[1] * tmp;
_normal[2] = center[2] * tmp + 8.f * coMath_f::pow2(m_radii[1]) * center[2];
_normal = normalize(_normal);
}
bool intersect_sphere(const ray3f& ray, const vec3f& o, float r, float& t) {
auto a = lengthSqr(ray.d);
auto b = 2*dot(ray.d,ray.e-o);
auto c = lengthSqr(ray.e-o) - r*r;
auto d = b*b-4*a*c;
if(d < 0) return false;
auto tmin = (-b-sqrt(d)) / (2*a);
auto tmax = (-b+sqrt(d)) / (2*a);
if (tmin >= ray.tmin && tmin <= ray.tmax) {
t = tmin;
return true;
}
if (tmax >= ray.tmin && tmax <= ray.tmax) {
t = tmax;
return true;
}
return false;
}
bool vertEq(const unsigned int index, const akMeshLoader::TempVert& b)
{
const akBufferInfo* vbi = item->getVertexBuffer();
akVector3* co, *no;
UTuint32* vcol;
float* uvlayer;
UTuint32 cos, nos, vcols, uvlayers;
if(vbi->getElement(akBufferInfo::BI_DU_VERTEX, akBufferInfo::VB_DT_3FLOAT32, 1, (void**)&co, &cos))
{
akAdvancePointer(co, cos * index);
if (!akFuzzyT(lengthSqr(co[0] - b.co), 1e-10f))
return false;
}
if(vbi->getElement(akBufferInfo::BI_DU_NORMAL, akBufferInfo::VB_DT_3FLOAT32, 1, (void**)&no, &nos))
{
akAdvancePointer(no, nos * index);
if (!akFuzzyT(lengthSqr(no[0] - b.no), 1e-10f))
return false;
}
if(vbi->getElement(akBufferInfo::BI_DU_COLOR, akBufferInfo::VB_DT_UINT32, 1, (void**)&vcol, &vcols))
{
akAdvancePointer(vcol, vcols * index);
if (vcol[0] != b.vcol)
return false;
}
for (unsigned int i = 0; i < item->getUVLayerCount(); i++)
{
if(vbi->getElement(akBufferInfo::BI_DU_UV, akBufferInfo::VB_DT_2FLOAT32, i+1, (void**)&uvlayer, &uvlayers))
{
float* uv = uvlayer;
akAdvancePointer(uv, uvlayers * index);;
akScalar d1 = uv[0] - b.uv[i][0];
akScalar d2 = uv[1] - b.uv[i][1];
if (!akFuzzyT(d1*d1+d2*d2, 1e-10f))
return false;
}
}
return true;
}
bool Sphere::intersectTest(const Sphere& other) const
{
vec3 axis(other.m_Position - m_Position);
float intersectDist(m_Radius + other.m_Radius);
if (lengthSqr(axis) < intersectDist * intersectDist)
return true;
return false;
}
akVector3 akMeshLoaderUtils_calcMorphNormal(const Blender::MFace& bface, float* pos)
{
akVector3 normal;
if(bface.v4 != 0)
{
akVector3 e0, e1;
akVector3 n1, n2;
akVector3 v0(pos[bface.v1*3], pos[bface.v1*3+1], pos[bface.v1*3+2]);
akVector3 v1(pos[bface.v2*3], pos[bface.v2*3+1], pos[bface.v2*3+2]);
akVector3 v2(pos[bface.v3*3], pos[bface.v3*3+1], pos[bface.v3*3+2]);
akVector3 v3(pos[bface.v4*3], pos[bface.v4*3+1], pos[bface.v4*3+2]);
e0 = v0 - v2;
e1 = v1 - v3;
if (lengthSqr(e0) <lengthSqr(e1))
{
n1 = akMeshLoaderUtils_calcNormal(v0,v1,v2);
n2 = akMeshLoaderUtils_calcNormal(v2,v3,v0);
}
else
{
n1 = akMeshLoaderUtils_calcNormal(v0, v1, v3);
n2 = akMeshLoaderUtils_calcNormal(v3, v1, v2);
}
normal = normalize(n1+n2);
}
else
{
akVector3 v0(pos[bface.v1*3], pos[bface.v1*3+1], pos[bface.v1*3+2]);
akVector3 v1(pos[bface.v2*3], pos[bface.v2*3+1], pos[bface.v2*3+2]);
akVector3 v2(pos[bface.v3*3], pos[bface.v3*3+1], pos[bface.v3*3+2]);
normal = akMeshLoaderUtils_calcNormal(v0,v1,v2);
}
return normal;
}
inline
PfxFloat
VertexBFaceATest(
PfxBool& inVoronoi,
PfxFloat& t0,
PfxFloat& t1,
PfxVector3& ptsVec,
const PfxVector3& hA,
PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG offsetAB,
PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG capsDirection,
PfxFloat signB,
PfxFloat scaleB )
{
// compute endpoint of capsule in box's coordinate system
PfxVector3 endpoint = PfxVector3( offsetAB + capsDirection * scaleB );
// compute the parameters of the point on the box face closest to this corner.
t0 = endpoint[0];
t1 = endpoint[1];
if ( t0 > hA[0] )
t0 = hA[0];
else if ( t0 < -hA[0] )
t0 = -hA[0];
if ( t1 > hA[1] )
t1 = hA[1];
else if ( t1 < -hA[1] )
t1 = -hA[1];
// get vector from face point to capsule endpoint
endpoint[0] -= t0;
endpoint[1] -= t1;
ptsVec = PfxVector3(endpoint);
// do the Voronoi test: already know the point on B is in the Voronoi region of the
// point on A, check the reverse.
inVoronoi = ( -signB * dot(ptsVec,capsDirection) >= voronoiTol );
return (lengthSqr(ptsVec));
}
bool Ray::IntersectDisk( v3 a_center, float a_radius, v3 a_normal )
{
const float vdotn = dot( m_dir, a_normal );
// the ray direction is parallel to triangle plane, return no intersection
if ( fabs(vdotn) <= EPSILON )
return false;
// signed distance to intersection point
const float sd = ( dot(a_center, a_normal) - dot(m_pos, a_normal) ) / vdotn;
if ( sd < 0.f )
return false;
const v4 i = m_pos + m_dir * sd;
if ( lengthSqr( i - a_center ) > a_radius*a_radius )
return false;
return NewHit( sd, a_normal );
}
inline static
bool operator ==(const PfxVector3 &a,const PfxVector3 &b)
{
return lengthSqr(a-b) < (SCE_PFX_GJK_EPSILON * SCE_PFX_GJK_EPSILON);
}
inline P_FLT Vector2D::length() const {
return sqrt(lengthSqr());
}
static quat invert(quat &q) {
return conjugate(q) / lengthSqr(q);
}
void addVertex(unsigned int fi, unsigned int bindex, const akMeshLoader::TempVert& ref)
{
utArray<float> uvs;
for(int j=0; j<AK_UV_MAX; j++)
{
uvs.push_back(ref.uv[j][0]);
uvs.push_back(ref.uv[j][1]);
}
UTuint32 id = item->addVertex(ref.co, ref.no, ref.vcol, uvs);
idxmap.push_back(bindex);
// vgroups
if(m_bmesh->dvert)
{
Blender::MDeformVert& dv = m_bmesh->dvert[bindex];
for(int j=0; j<dv.totweight; j++)
{
UTuint32 vgi = dv.dw[j].def_nr;
if( vgi < item->getNumVertexGroups() )
{
akVertexGroup* vg = item->getVertexGroup(vgi);
vg->add(id, dv.dw[j].weight);
}
}
}
// morphtargets
if(m_bmesh->key)
{
Blender::KeyBlock* bkb = (Blender::KeyBlock*)m_bmesh->key->block.first;
// skip first shape key (basis)
int mti=0;
if(bkb) bkb = bkb->next;
while(bkb)
{
if(bkb->type == KEY_RELATIVE)
{
Blender::KeyBlock* basis = (Blender::KeyBlock*)m_bmesh->key->block.first;
for(int i=0; basis && i<bkb->relative; i++)
basis = basis->next;
if(basis)
{
//akMorphTarget* mt = item->getMorphTarget(bkb->name);
akMorphTarget* mt = item->getMorphTarget(mti);
float* kpos = (float*)bkb->data;
float* bpos = (float*)basis->data;
akVector3 k(kpos[3*bindex+0], kpos[3*bindex+1], kpos[3*bindex+2]);
akVector3 b(bpos[3*bindex+0], bpos[3*bindex+1], bpos[3*bindex+2]);
k = k-b;
btAlignedObjectArray<akVector3>& norms = shapekeysnormals->at(mti);
akVector3 normal(0,0,0);
const Blender::MFace& bface = m_bmesh->mface[fi];
if(bface.flag & ME_SMOOTH)
{
utArray<UTuint32>& smoothfaces = smoothfacesarray->at(bindex);
for (int j = 0; j< smoothfaces.size(); j++)
{
UTuint32 bface2id = smoothfaces[j];
normal += norms.at(bface2id);
}
normal = normalize(normal);
}
else
{
normal = norms.at(fi);
}
normal = normal - ref.no;
if(!akFuzzyT(lengthSqr(k), 1e-10f) || !akFuzzyT(lengthSqr(normal), 1e-10f))
mt->add(id, k, normal);
mti++;
}
}
bkb = bkb->next;
}
}
}
void Physics::SolveConstraints(
RigidbodyState *states,
const RigidBodyElements *bodies,
unsigned int numRigidBodies,const Pair *pairs,
unsigned int numPairs,
BallJoint *joints,unsigned int numJoints,
unsigned int iteration, float bias,float slop,
float timeStep,Allocator *allocator)
{
assert(states);
assert(bodies);
assert(pairs);
// ソルバー用プロキシを作成
SolverBody *solverBodies = (SolverBody*)allocator->allocate(sizeof(SolverBody)*numRigidBodies);
assert(solverBodies);
for (unsigned int i = 0; i<numRigidBodies; i++) {
RigidbodyState &state = states[i];
const RigidBodyElements &body = bodies[i];
SolverBody &solverBody = solverBodies[i];
solverBody.orientation = state.m_orientation;
solverBody.deltaLinearVelocity = Vector3(0.0f);
solverBody.deltaAngularVelocity = Vector3(0.0f);
if (state.m_motionType == MotionType::TypeStatic) {
solverBody.massInv = 0.0f;
solverBody.inertiaInv = Matrix3(0.0f);
}
else {
solverBody.massInv = 1.0f / body.m_mass;
Matrix3 m(solverBody.orientation);
solverBody.inertiaInv = m * inverse(body.m_inertia) * transpose(m);
}
}
// 拘束のセットアップ
for (unsigned int i = 0; i<numJoints; i++) {
BallJoint &joint = joints[i];
RigidbodyState &stateA = states[joint.rigidBodyA];
const RigidBodyElements &bodyA = bodies[joint.rigidBodyA];
SolverBody &solverBodyA = solverBodies[joint.rigidBodyA];
RigidbodyState &stateB = states[joint.rigidBodyB];
const RigidBodyElements &bodyB = bodies[joint.rigidBodyB];
SolverBody &solverBodyB = solverBodies[joint.rigidBodyB];
Vector3 rA = rotate(solverBodyA.orientation, joint.anchorA);
Vector3 rB = rotate(solverBodyB.orientation, joint.anchorB);
Vector3 positionA = stateA.m_position + rA;
Vector3 positionB = stateB.m_position + rB;
Vector3 direction = positionA - positionB;
float distanceSqr = lengthSqr(direction);
if (distanceSqr < EPSILON * EPSILON) {
joint.constraint.jacDiagInv = 0.0f;
joint.constraint.rhs = 0.0f;
joint.constraint.lowerLimit = -FLT_MAX;
joint.constraint.upperLimit = FLT_MAX;
joint.constraint.axis = Vector3(1.0f, 0.0f, 0.0f);
continue;
}
float distance = sqrtf(distanceSqr);
direction /= distance;
Vector3 velocityA = stateA.m_linearVelocity + cross(stateA.m_angularVelocity, rA);
Vector3 velocityB = stateB.m_linearVelocity + cross(stateB.m_angularVelocity, rB);
Vector3 relativeVelocity = velocityA - velocityB;
Matrix3 K = Matrix3::scale(Vector3(solverBodyA.massInv + solverBodyB.massInv)) -
crossMatrix(rA) * solverBodyA.inertiaInv * crossMatrix(rA) -
crossMatrix(rB) * solverBodyB.inertiaInv * crossMatrix(rB);
float denom = dot(K * direction, direction);
joint.constraint.jacDiagInv = 1.0f / denom;
joint.constraint.rhs = -dot(relativeVelocity, direction); // velocity error
joint.constraint.rhs -= joint.bias * distance / timeStep; // position error
joint.constraint.rhs *= joint.constraint.jacDiagInv;
joint.constraint.lowerLimit = -FLT_MAX;
joint.constraint.upperLimit = FLT_MAX;
joint.constraint.axis = direction;
joint.constraint.accumImpulse = 0.0f;
}
for (unsigned int i = 0; i<numPairs; i++) {
const Pair &pair = pairs[i];
RigidbodyState &stateA = states[pair.rigidBodyA];
const RigidBodyElements &bodyA = bodies[pair.rigidBodyA];
SolverBody &solverBodyA = solverBodies[pair.rigidBodyA];
RigidbodyState &stateB = states[pair.rigidBodyB];
const RigidBodyElements &bodyB = bodies[pair.rigidBodyB];
SolverBody &solverBodyB = solverBodies[pair.rigidBodyB];
assert(pair.contact);
pair.contact->m_friction = sqrtf(bodyA.m_friction * bodyB.m_friction);
for (unsigned int j = 0; j<pair.contact->m_numContacts; j++) {
ContactPoint &cp = pair.contact->m_contactPoints[j];
Vector3 rA = rotate(solverBodyA.orientation, cp.pointA);
Vector3 rB = rotate(solverBodyB.orientation, cp.pointB);
Matrix3 K = Matrix3::scale(Vector3(solverBodyA.massInv + solverBodyB.massInv)) -
crossMatrix(rA) * solverBodyA.inertiaInv * crossMatrix(rA) -
crossMatrix(rB) * solverBodyB.inertiaInv * crossMatrix(rB);
Vector3 velocityA = stateA.m_linearVelocity + cross(stateA.m_angularVelocity, rA);
Vector3 velocityB = stateB.m_linearVelocity + cross(stateB.m_angularVelocity, rB);
Vector3 relativeVelocity = velocityA - velocityB;
Vector3 tangent1, tangent2;
CalcTangentVector(cp.normal, tangent1, tangent2);
float restitution = pair.type == PairTypeNew ? 0.5f*(bodyA.m_restitution + bodyB.m_restitution) : 0.0f;
// Normal
{
Vector3 axis = cp.normal;
float denom = dot(K * axis, axis);
cp.constraints[0].jacDiagInv = 1.0f / denom;
cp.constraints[0].rhs = -(1.0f + restitution) * dot(relativeVelocity, axis); // velocity error
cp.constraints[0].rhs -= (bias * MIN(0.0f, cp.distance + slop)) / timeStep; // position error
cp.constraints[0].rhs *= cp.constraints[0].jacDiagInv;
cp.constraints[0].lowerLimit = 0.0f;
cp.constraints[0].upperLimit = FLT_MAX;
cp.constraints[0].axis = axis;
}
// Tangent1
{
Vector3 axis = tangent1;
float denom = dot(K * axis, axis);
cp.constraints[1].jacDiagInv = 1.0f / denom;
cp.constraints[1].rhs = -dot(relativeVelocity, axis);
cp.constraints[1].rhs *= cp.constraints[1].jacDiagInv;
cp.constraints[1].lowerLimit = 0.0f;
cp.constraints[1].upperLimit = 0.0f;
cp.constraints[1].axis = axis;
}
// Tangent2
{
Vector3 axis = tangent2;
float denom = dot(K * axis, axis);
cp.constraints[2].jacDiagInv = 1.0f / denom;
cp.constraints[2].rhs = -dot(relativeVelocity, axis);
cp.constraints[2].rhs *= cp.constraints[2].jacDiagInv;
cp.constraints[2].lowerLimit = 0.0f;
cp.constraints[2].upperLimit = 0.0f;
cp.constraints[2].axis = axis;
}
}
}
// Warm starting
for (unsigned int i = 0; i<numPairs; i++) {
const Pair &pair = pairs[i];
SolverBody &solverBodyA = solverBodies[pair.rigidBodyA];
SolverBody &solverBodyB = solverBodies[pair.rigidBodyB];
for (unsigned int j = 0; j<pair.contact->m_numContacts; j++) {
ContactPoint &cp = pair.contact->m_contactPoints[j];
Vector3 rA = rotate(solverBodyA.orientation, cp.pointA);
Vector3 rB = rotate(solverBodyB.orientation, cp.pointB);
for (unsigned int k = 0; k<3; k++) {
float deltaImpulse = cp.constraints[k].accumImpulse;
solverBodyA.deltaLinearVelocity += deltaImpulse * solverBodyA.massInv * cp.constraints[k].axis;
solverBodyA.deltaAngularVelocity += deltaImpulse * solverBodyA.inertiaInv * cross(rA, cp.constraints[k].axis);
solverBodyB.deltaLinearVelocity -= deltaImpulse * solverBodyB.massInv * cp.constraints[k].axis;
solverBodyB.deltaAngularVelocity -= deltaImpulse * solverBodyB.inertiaInv * cross(rB, cp.constraints[k].axis);
}
}
}
// 拘束の演算
for (unsigned int itr = 0; itr<iteration; itr++) {
for (unsigned int i = 0; i<numJoints; i++) {
BallJoint &joint = joints[i];
SolverBody &solverBodyA = solverBodies[joint.rigidBodyA];
SolverBody &solverBodyB = solverBodies[joint.rigidBodyB];
Vector3 rA = rotate(solverBodyA.orientation, joint.anchorA);
Vector3 rB = rotate(solverBodyB.orientation, joint.anchorB);
Constraint &constraint = joint.constraint;
float deltaImpulse = constraint.rhs;
Vector3 deltaVelocityA = solverBodyA.deltaLinearVelocity + cross(solverBodyA.deltaAngularVelocity, rA);
Vector3 deltaVelocityB = solverBodyB.deltaLinearVelocity + cross(solverBodyB.deltaAngularVelocity, rB);
deltaImpulse -= constraint.jacDiagInv * dot(constraint.axis, deltaVelocityA - deltaVelocityB);
float oldImpulse = constraint.accumImpulse;
constraint.accumImpulse = CLAMP(oldImpulse + deltaImpulse, constraint.lowerLimit, constraint.upperLimit);
deltaImpulse = constraint.accumImpulse - oldImpulse;
solverBodyA.deltaLinearVelocity += deltaImpulse * solverBodyA.massInv * constraint.axis;
solverBodyA.deltaAngularVelocity += deltaImpulse * solverBodyA.inertiaInv * cross(rA, constraint.axis);
solverBodyB.deltaLinearVelocity -= deltaImpulse * solverBodyB.massInv * constraint.axis;
solverBodyB.deltaAngularVelocity -= deltaImpulse * solverBodyB.inertiaInv * cross(rB, constraint.axis);
}
for (unsigned int i = 0; i<numPairs; i++) {
const Pair &pair = pairs[i];
SolverBody &solverBodyA = solverBodies[pair.rigidBodyA];
SolverBody &solverBodyB = solverBodies[pair.rigidBodyB];
for (unsigned int j = 0; j<pair.contact->m_numContacts; j++) {
ContactPoint &cp = pair.contact->m_contactPoints[j];
Vector3 rA = rotate(solverBodyA.orientation, cp.pointA);
Vector3 rB = rotate(solverBodyB.orientation, cp.pointB);
{
Constraint &constraint = cp.constraints[0];
float deltaImpulse = constraint.rhs;
Vector3 deltaVelocityA = solverBodyA.deltaLinearVelocity + cross(solverBodyA.deltaAngularVelocity, rA);
Vector3 deltaVelocityB = solverBodyB.deltaLinearVelocity + cross(solverBodyB.deltaAngularVelocity, rB);
deltaImpulse -= constraint.jacDiagInv * dot(constraint.axis, deltaVelocityA - deltaVelocityB);
float oldImpulse = constraint.accumImpulse;
constraint.accumImpulse = CLAMP(oldImpulse + deltaImpulse, constraint.lowerLimit, constraint.upperLimit);
deltaImpulse = constraint.accumImpulse - oldImpulse;
solverBodyA.deltaLinearVelocity += deltaImpulse * solverBodyA.massInv * constraint.axis;
solverBodyA.deltaAngularVelocity += deltaImpulse * solverBodyA.inertiaInv * cross(rA, constraint.axis);
solverBodyB.deltaLinearVelocity -= deltaImpulse * solverBodyB.massInv * constraint.axis;
solverBodyB.deltaAngularVelocity -= deltaImpulse * solverBodyB.inertiaInv * cross(rB, constraint.axis);
}
float maxFriction = pair.contact->m_friction * fabs(cp.constraints[0].accumImpulse);
cp.constraints[1].lowerLimit = -maxFriction;
cp.constraints[1].upperLimit = maxFriction;
cp.constraints[2].lowerLimit = -maxFriction;
cp.constraints[2].upperLimit = maxFriction;
{
Constraint &constraint = cp.constraints[1];
float deltaImpulse = constraint.rhs;
Vector3 deltaVelocityA = solverBodyA.deltaLinearVelocity + cross(solverBodyA.deltaAngularVelocity, rA);
Vector3 deltaVelocityB = solverBodyB.deltaLinearVelocity + cross(solverBodyB.deltaAngularVelocity, rB);
deltaImpulse -= constraint.jacDiagInv * dot(constraint.axis, deltaVelocityA - deltaVelocityB);
float oldImpulse = constraint.accumImpulse;
constraint.accumImpulse = CLAMP(oldImpulse + deltaImpulse, constraint.lowerLimit, constraint.upperLimit);
deltaImpulse = constraint.accumImpulse - oldImpulse;
solverBodyA.deltaLinearVelocity += deltaImpulse * solverBodyA.massInv * constraint.axis;
solverBodyA.deltaAngularVelocity += deltaImpulse * solverBodyA.inertiaInv * cross(rA, constraint.axis);
solverBodyB.deltaLinearVelocity -= deltaImpulse * solverBodyB.massInv * constraint.axis;
solverBodyB.deltaAngularVelocity -= deltaImpulse * solverBodyB.inertiaInv * cross(rB, constraint.axis);
}
{
Constraint &constraint = cp.constraints[2];
float deltaImpulse = constraint.rhs;
Vector3 deltaVelocityA = solverBodyA.deltaLinearVelocity + cross(solverBodyA.deltaAngularVelocity, rA);
Vector3 deltaVelocityB = solverBodyB.deltaLinearVelocity + cross(solverBodyB.deltaAngularVelocity, rB);
deltaImpulse -= constraint.jacDiagInv * dot(constraint.axis, deltaVelocityA - deltaVelocityB);
float oldImpulse = constraint.accumImpulse;
constraint.accumImpulse = CLAMP(oldImpulse + deltaImpulse, constraint.lowerLimit, constraint.upperLimit);
deltaImpulse = constraint.accumImpulse - oldImpulse;
solverBodyA.deltaLinearVelocity += deltaImpulse * solverBodyA.massInv * constraint.axis;
solverBodyA.deltaAngularVelocity += deltaImpulse * solverBodyA.inertiaInv * cross(rA, constraint.axis);
solverBodyB.deltaLinearVelocity -= deltaImpulse * solverBodyB.massInv * constraint.axis;
solverBodyB.deltaAngularVelocity -= deltaImpulse * solverBodyB.inertiaInv * cross(rB, constraint.axis);
}
}
}
}
// 速度を更新
for (unsigned int i = 0; i<numRigidBodies; i++) {
states[i].m_linearVelocity += solverBodies[i].deltaLinearVelocity;
states[i].m_angularVelocity += solverBodies[i].deltaAngularVelocity;
}
allocator->deallocate(solverBodies);
}
/// Returns the length of this vector
inline double length() const {
return sqrt(lengthSqr());
}
void circleToGrid(phys::Collision* c, phys::RigidBody *a, phys::RigidBody *b)
{
GridShape* grid = reinterpret_cast<GridShape*>(b->getShape());
phys::CircleShape* circle = reinterpret_cast<phys::CircleShape*>(a->getShape());
// Transform circle center to Polygon model space
sf::Vector2f center = a->getPosition();
center = grid->getU().transpose() * (center - b->getPosition());
sf::Vector2f topLeft(center.x-circle->getRadius(), center.y-circle->getRadius());
sf::Vector2f botRight(center.x+circle->getRadius(), center.y+circle->getRadius());
int left = floor(topLeft.x/(PTU/TILE_SIZE))-1;
int top = floor(topLeft.y/(PTU/TILE_SIZE))-1;
int right = ceil(botRight.x/(PTU/TILE_SIZE))+1;
int bot = ceil(botRight.y/(PTU/TILE_SIZE))+1;
if (!grid->getGrid()->getWrapX())
{
left = std::max(left, 0);
right = std::min(right, grid->getGrid()->getSizeX()-1);
}
top = std::max(top, 0);
bot = std::min(bot, grid->getGrid()->getSizeY()-1);
// Tile size
float tileSize = PTU/TILE_SIZE;
// Find the tile with the least overlap in the collision
sf::Vector2f tileVerts[4];
int tileVertCount;
sf::Vector2f tileNormals[4];
sf::Vector2f usedNormals[20];
int usedNormalCount = 0;
for (int y = top; y <= bot; y++)
{
for (int _x = left; _x <= right; _x++)
{
c->addManifold();
c->getLastManifold()->contactCount = 0;
int x = _x;
if (grid->getGrid()->getWrapX())
x = grid->getGrid()->wrapX(x);
if (!grid->getGrid()->mTiles[y][x].isSolid())
continue;
int left = x-1;
int right = x+1;
int up = y-1;
int down = y+1;
if (grid->getGrid()->mWrapX)
{
left = grid->getGrid()->wrapX(left);
right = grid->getGrid()->wrapX(right);
}
bool leftT = false;
bool rightT = false;
bool upT = false;
bool downT = false;
if (left >= 0 && grid->getGrid()->mTiles[y][left].isSolid())
leftT = true;
if (right < grid->getGrid()->mSizeX && grid->getGrid()->mTiles[y][right].isSolid())
rightT = true;
if (up >= 0 && grid->getGrid()->mTiles[up][x].isSolid())
upT = true;
if (down < grid->getGrid()->mSizeY && grid->getGrid()->mTiles[down][x].isSolid())
downT = true;
sf::Vector2f start(_x*tileSize, y*tileSize);
if (!leftT && !rightT && !upT && downT)
{
tileVertCount = 3;
tileVerts[0] = start + sf::Vector2f(tileSize/2, 0);
tileVerts[1] = start + sf::Vector2f(0, tileSize);
tileVerts[2] = start + sf::Vector2f(tileSize, tileSize);
tileNormals[0] = normalize(sf::Vector2f(-0.5, -1.f));
tileNormals[1] = sf::Vector2f(0, 1);
tileNormals[2] = normalize(sf::Vector2f(0.5, -1.f));
}
else if (!leftT && rightT && !upT && downT)
{
tileVertCount = 3;
tileVerts[0] = start + sf::Vector2f(tileSize, 0);
tileVerts[1] = start + sf::Vector2f(0, tileSize);
tileVerts[2] = start + sf::Vector2f(tileSize, tileSize);
tileNormals[0] = normalize(sf::Vector2f(-1.f, -1.f));
tileNormals[1] = sf::Vector2f(0, 1);
tileNormals[2] = sf::Vector2f(1.f, 0.f);
}
else if (leftT && !rightT && !upT && downT)
{
tileVertCount = 3;
tileVerts[0] = start;
tileVerts[1] = start + sf::Vector2f(0, tileSize);
tileVerts[2] = start + sf::Vector2f(tileSize, tileSize);
tileNormals[0] = sf::Vector2f(-1.f, 0.f);
tileNormals[1] = sf::Vector2f(0, 1);
tileNormals[2] = normalize(sf::Vector2f(1.f, -1.f));
}
else
{
tileVertCount = 4;
tileVerts[0] = start;
tileVerts[1] = start + sf::Vector2f(0, tileSize);
tileVerts[2] = start + sf::Vector2f(tileSize, tileSize);
tileVerts[3] = start + sf::Vector2f(tileSize, 0);
tileNormals[0] = sf::Vector2f(-1, 0);
tileNormals[1] = sf::Vector2f(0, 1);
tileNormals[2] = sf::Vector2f(1, 0);
tileNormals[3] = sf::Vector2f(0, -1);
}
// Find edge with minimum penetration
// Exact concept as using support points in Polygon vs Polygon
float separation = -FLT_MAX;
sf::Uint32 faceNormal = 0;
bool done = false;
for(sf::Uint32 i = 0; i < tileVertCount; ++i)
{
float s = dot( tileNormals[i], center - tileVerts[i] );
if(s > circle->getRadius())
{
done = true;
break;
}
if(s > separation)
{
separation = s;
faceNormal = i;
}
}
if (done)
continue;
/*bool used = false;
for (int i = 0; i < usedNormalCount; i++)
{
if (tileNormals[faceNormal] == usedNormals[i])
{
used = true;
break;
}
}
if (used)
continue;
usedNormals[usedNormalCount] = tileNormals[faceNormal];
usedNormalCount++;*/
// Grab face's vertices
sf::Vector2f v1 = tileVerts[faceNormal];
sf::Uint32 i2 = faceNormal + 1 < tileVertCount ? faceNormal + 1 : 0;
sf::Vector2f v2 = tileVerts[i2];
// Check to see if center is within polygon
if(separation < EPSILON)
{
c->getLastManifold()->contactCount = 1;
c->getLastManifold()->normal = -(grid->getU() * tileNormals[faceNormal]);
c->getLastManifold()->contacts[0] = c->getLastManifold()->normal * circle->getRadius() + b->getPosition();
c->getLastManifold()->penetration = circle->getRadius();
continue;
}
// Determine which voronoi region of the edge center of circle lies within
float dot1 = dot( center - v1, v2 - v1 );
float dot2 = dot( center - v2, v1 - v2 );
c->getLastManifold()->penetration = circle->getRadius() - separation;
// Closest to v1
if(dot1 <= 0.0f)
{
if(lengthSqr( center - v1 ) > circle->getRadius() * circle->getRadius())
continue;
c->getLastManifold()->contactCount = 1;
sf::Vector2f n = v1 - center;
n = grid->getU() * n;
n = normalize(n);
c->getLastManifold()->normal = n;
v1 = grid->getU() * v1 + a->getPosition();
c->getLastManifold()->contacts[0] = v1;
}
// Closest to v2
else if(dot2 <= 0.0f)
{
if(lengthSqr( center - v2 ) > circle->getRadius() * circle->getRadius())
continue;
c->getLastManifold()->contactCount = 1;
sf::Vector2f n = v2 - center;
v2 = grid->getU() * v2 + a->getPosition();
c->getLastManifold()->contacts[0] = v2;
n = grid->getU() * n;
n = normalize(n);
c->getLastManifold()->normal = n;
}
// Closest to face
else
{
sf::Vector2f n = tileNormals[faceNormal];
if(dot( center - v1, n ) > circle->getRadius())
continue;
n = grid->getU() * n;
c->getLastManifold()->normal = -n;
c->getLastManifold()->contacts[0] = c->getLastManifold()->normal * circle->getRadius() + b->getPosition();
c->getLastManifold()->contactCount = 1;
}
}
}
}
// particle simulation
void simulate(Scene* scene) {
//put_your_code_here("Implement simulation");
// for each mesh
for(auto m : scene->meshes){
// skip if no simulation
if(!m->simulation){continue;}
// compute time per step
float timeperstep = scene->animation->dt/scene->animation->simsteps;
// foreach simulation steps
for(int step = 0; step < scene->animation->simsteps; step++){
// foreach particle, compute external forces
for(int particle: range(0, m->simulation->force.size())){
// initialize particle forces to zero3f
m->simulation->force[particle] = zero3f;
// compute force of gravity
auto g = scene->animation->gravity;
// compute force of wind
//EXTRA CREDIT DON"T DO WIND
// accumulate sum of forces on particle
m->simulation->force[particle] += g;
}
//SPRING EXTRA CREDIT
// for each spring, compute spring force on points
// compute spring distance and length
// compute static force
// accumulate static force on points
// compute dynamic force
// accumulate dynamic force on points
// foreach particle, integrate using newton laws
for(int particle: range(0, m->simulation->force.size())){
// if pinned, skip
if(m->simulation->pinned[particle]){continue;}
// compute acceleration
auto acceleration = m->simulation->force[particle]/m->simulation->mass[particle];
// update velocity and positions using Euler's method
m->simulation->vel[particle] = m->simulation->vel[particle] + acceleration*timeperstep ;
m->pos[particle] = m->pos[particle] + m->simulation->vel[particle]*timeperstep + acceleration*timeperstep*timeperstep*.5;
// for each surface, check for collision
bool inside;
for(auto surface : scene->surfaces){
inside = false;
// compute local position
auto L_pos = transform_point_to_local(surface->frame, m->pos[particle]);
auto R = surface->radius;
// if quad
vec3f col_pos;
vec3f col_norm;
if(surface->isquad){
// perform inside test
if(L_pos.x < R && L_pos.x > -R && L_pos.z < 0 && L_pos.y < R && L_pos.y > -R){
inside = true;
// vec3f col_pos = m->pos[particle];
col_norm = surface->frame.z;
col_pos = transform_point_from_local(surface->frame, vec3f(L_pos.x, L_pos.y, 0));
}
}
// if sphere
if(!surface->isquad){
// inside test
if(dist(zero3f, L_pos) < R){
// // if inside, compute a collision position and normal
inside = true;
// vec3f col_pos = m->pos[particle];
vec3f C = surface->frame.o; //sphere center
vec3f E = surface->frame.o; //camera origin
float a = lengthSqr(C - m->pos[particle]);
float b = dot(2*(C - m->pos[particle]), C-E);
float c = lengthSqr(C - m->pos[particle])-sqr(R);
float det = sqr(b) - 4*a*c;
//if(det < 0){continue;}
//calculate t1 and t2
float t1 = (-b + sqrt(det))/(2*a);
float t2 = (-b - sqrt(det))/(2*a);
// just grab only the first hit
float t = fminf(t1, t2);
//check if computed param is within ray.tmin and ray.tmax
col_pos = m->pos[particle] + t*(C - m->pos[particle]); //find intersection point with quad
col_norm = normalize(col_pos - C);
}
}
// if inside
if(inside == true){
m->pos[particle]=col_pos;
// update velocity (particle bounces), taking into account loss of kinetic energy
vec3f ref = reflect(m->simulation->vel[particle], col_norm);
auto v1 = ref - dot(ref, col_norm)*col_norm;
auto v2 = dot(ref, col_norm)*col_norm;
m->simulation->vel[particle] = (1 - scene->animation->bounce_dump.x)*v1 +(1 - scene->animation->bounce_dump.y)*v2;
}
}
}
}
// smooth normals if it has triangles or quads
if(m->quad.size() != 0 || m->triangle.size() != 0){
smooth_normals(m);
}
}
}
EpxBool epxConvexConvexContact_local(
const EpxConvexMesh &convexA,const EpxTransform3 &transformA,
const EpxConvexMesh &convexB,const EpxTransform3 &transformB,
EpxVector3 &normal,
EpxFloat &penetrationDepth,
EpxVector3 &contactPointA,
EpxVector3 &contactPointB)
{
EpxTransform3 transformAB,transformBA;
EpxMatrix3 matrixAB,matrixBA;
EpxVector3 offsetAB,offsetBA;
// Bローカル→Aローカルへの変換
transformAB = orthoInverse(transformA) * transformB;
matrixAB = transformAB.getUpper3x3();
offsetAB = transformAB.getTranslation();
// Aローカル→Bローカルへの変換
transformBA = orthoInverse(transformAB);
matrixBA = transformBA.getUpper3x3();
offsetBA = transformBA.getTranslation();
// 最も浅い貫通深度とそのときの分離軸
EpxFloat distanceMin = -EPX_FLT_MAX;
EpxVector3 axisMin(0.0f);
EpxSatType satType = EpxSatTypeEdgeEdge;
EpxBool axisFlip;
//----------------------------------------------------------------------------
// 分離軸判定
int satCount = 0;
// 面法線の判定を優先させたいので、エッジ外積→面法線の順に判定
// ConvexAとConvexBの外積を分離軸とする
EpxUInt32 edgeIdMinA,edgeIdMinB;
for(EpxUInt32 eA=0;eA<convexA.m_numEdges;eA++) {
const EpxEdge &edgeA = convexA.m_edges[eA];
if(edgeA.type != EpxEdgeTypeConvex) continue;
const EpxVector3 edgeVecA = convexA.m_vertices[edgeA.vertId[1]] - convexA.m_vertices[edgeA.vertId[0]];
for(EpxUInt32 eB=0;eB<convexB.m_numEdges;eB++) {
const EpxEdge &edgeB = convexB.m_edges[eB];
if(edgeB.type != EpxEdgeTypeConvex) continue;
const EpxVector3 edgeVecB = matrixAB * (convexB.m_vertices[edgeB.vertId[1]] - convexB.m_vertices[edgeB.vertId[0]]);
// Gauss map algorithm from GDC 2013 physics tutorial
EpxVector3 eA0 = convexA.m_facets[edgeA.facetId[0]].normal;
EpxVector3 eA1 = convexA.m_facets[edgeA.facetId[1]].normal;
EpxVector3 eB0 = -matrixAB * convexB.m_facets[edgeB.facetId[0]].normal;
EpxVector3 eB1 = -matrixAB * convexB.m_facets[edgeB.facetId[1]].normal;
if(!isValidEdge(eA0,eA1,eB0,eB1)) continue;
EpxVector3 separatingAxis = cross(edgeVecA,edgeVecB);
if(lengthSqr(separatingAxis) < EPX_EPSILON*EPX_EPSILON) continue;
separatingAxis = normalize(separatingAxis);
EpxVector3 pA = convexA.m_vertices[edgeA.vertId[0]];
EpxVector3 pB = offsetAB + matrixAB * convexB.m_vertices[edgeB.vertId[0]];
if(dot(separatingAxis,pA) > 0.0f) { // 原点は必ずConvexの内側に存在すること
separatingAxis = -separatingAxis;
}
EpxFloat d = dot(separatingAxis,pA - pB);
satCount++;
if(d >= 0.0f) {
return false;
}
if(distanceMin < d) {
distanceMin = d;
axisMin = separatingAxis;
satType = EpxSatTypeEdgeEdge;
edgeIdMinA = eA;
edgeIdMinB = eB;
}
}
}
// ConvexAの面法線を分離軸とする
for(EpxUInt32 f=0;f<convexA.m_numFacets;f++) {
const EpxFacet &facet = convexA.m_facets[f];
const EpxVector3 separatingAxis = facet.normal;
EpxVector3 axisB = matrixBA * separatingAxis;
EpxVector3 pA = offsetBA + matrixBA * convexA.m_vertices[facet.vertId[0]];
EpxFloat minB = EPX_FLT_MAX;
for(EpxUInt32 i=0;i<convexB.m_numVertices;i++) {
EpxFloat prj = dot(axisB,convexB.m_vertices[i] - pA);
minB = EPX_MIN(minB,prj);
}
satCount++;
if(minB >= 0.0f) {
return false;
}
if(distanceMin < minB) {
distanceMin = minB;
axisMin = -separatingAxis;
satType = EpxSatTypePointBFacetA;
axisFlip = true;
}
}
// ConvexBの面法線を分離軸とする
for(EpxUInt32 f=0;f<convexB.m_numFacets;f++) {
const EpxFacet &facet = convexB.m_facets[f];
const EpxVector3 separatingAxis = matrixAB * facet.normal;
EpxVector3 pB = offsetAB + matrixAB * convexB.m_vertices[facet.vertId[0]];
EpxFloat minA = EPX_FLT_MAX;
for(EpxUInt32 i=0;i<convexA.m_numVertices;i++) {
EpxFloat prj = dot(separatingAxis,convexA.m_vertices[i] - pB);
minA = EPX_MIN(minA,prj);
}
satCount++;
if(minA >= 0.0f) {
return false;
}
if(distanceMin < minA) {
distanceMin = minA;
axisMin = separatingAxis;
satType = EpxSatTypePointAFacetB;
axisFlip = false;
}
}
// ここまで到達したので、２つの凸メッシュは交差している。
// また、反発ベクトル(axisMin)と貫通深度(distanceMin)が求まった。
// 反発ベクトルはＡを押しだす方向をプラスにとる。
//int satTotal = convexA.m_numFacets + convexB.m_numFacets + convexA.m_numEdges * convexB.m_numEdges;
//epxPrintf("sat check count %d / %d\n",satCount,satTotal);
//----------------------------------------------------------------------------
// 衝突座標検出
int collCount = 0;
EpxFloat closestMinSqr = EPX_FLT_MAX;
EpxVector3 closestPointA,closestPointB;
EpxVector3 separation = 1.1f * fabs(distanceMin) * axisMin;
if(satType == EpxSatTypeEdgeEdge) {
const EpxEdge &edgeA = convexA.m_edges[edgeIdMinA];
const EpxEdge &edgeB = convexB.m_edges[edgeIdMinB];
EpxVector3 sA,sB;
epxGetClosestTwoSegments(
separation + convexA.m_vertices[edgeA.vertId[0]],
separation + convexA.m_vertices[edgeA.vertId[1]],
offsetAB + matrixAB * convexB.m_vertices[edgeB.vertId[0]],
offsetAB + matrixAB * convexB.m_vertices[edgeB.vertId[1]],
sA,sB);
EpxFloat dSqr = lengthSqr(sA-sB);
closestMinSqr = dSqr;
closestPointA = sA;
closestPointB = sB;
collCount++;
}
else {
// 分離平面を挟んで向かい合う面を抽出
EpxUInt8 facetsA[EPX_CONVEX_MESH_MAX_FACETS];
EpxUInt8 facetsB[EPX_CONVEX_MESH_MAX_FACETS];
EpxUInt8 numFacetsA = 0;
EpxUInt8 numFacetsB = 0;
if(satType == EpxSatTypePointBFacetA) {
for(EpxUInt8 fA=0;fA<convexA.m_numFacets;fA++) {
const EpxFacet &facetA = convexA.m_facets[fA];
EpxFloat checkA = dot(facetA.normal,-axisMin);
if(checkA < 0.99f && axisFlip) {
// 判定軸が面Aの法線のとき、向きの違うAの面は判定しない
continue;
}
if(checkA < 0.0f) {
// 衝突面と逆に向いている面は判定しない
continue;
}
facetsA[numFacetsA++] = (EpxUInt8)fA;
}
EpxFloat checkBMax = -1.0f;
for(EpxUInt8 fB=0;fB<convexB.m_numFacets;fB++) {
const EpxFacet &facetB = convexB.m_facets[fB];
EpxFloat checkB = dot(facetB.normal,matrixBA * axisMin);
if(checkB > checkBMax) {
checkBMax = checkB;
facetsB[0] = fB;
numFacetsB = 1;
}
else if(checkB > checkBMax - EPX_EPSILON) { // checkB == checkBMax
facetsB[numFacetsB++] = fB;
}
}
}
else { // satType == EpxSatTypePointAFacetB
for(EpxUInt8 fB=0;fB<convexB.m_numFacets;fB++) {
const EpxFacet &facetB = convexB.m_facets[fB];
EpxFloat checkB = dot(facetB.normal,matrixBA * axisMin);
if(checkB < 0.99f && !axisFlip) {
// 判定軸が面Bの法線のとき、向きの違うBの面は判定しない
continue;
}
if(checkB < 0.0f) {
// 衝突面と逆に向いている面は判定しない
continue;
}
facetsB[numFacetsB++] = (EpxUInt8)fB;
}
EpxFloat checkAMax = -1.0f;
for(EpxUInt8 fA=0;fA<convexA.m_numFacets;fA++) {
const EpxFacet &facetA = convexA.m_facets[fA];
EpxFloat checkA = dot(facetA.normal,-axisMin);
if(checkA > checkAMax) {
checkAMax = checkA;
facetsA[0] = fA;
numFacetsA = 1;
}
else if(checkA > checkAMax - EPX_EPSILON) { // checkA == checkAMax
facetsA[numFacetsA++] = fA;
}
}
}
for(EpxUInt8 fA=0;fA<numFacetsA;fA++) {
const EpxFacet &facetA = convexA.m_facets[facetsA[fA]];
for(EpxUInt8 fB=0;fB<numFacetsB;fB++) {
const EpxFacet &facetB = convexB.m_facets[facetsB[fB]];
collCount++;
// 面Ａと面Ｂの最近接点を求める
EpxVector3 triangleA[3] = {
separation + convexA.m_vertices[facetA.vertId[0]],
separation + convexA.m_vertices[facetA.vertId[1]],
separation + convexA.m_vertices[facetA.vertId[2]],
};
EpxVector3 triangleB[3] = {
offsetAB + matrixAB * convexB.m_vertices[facetB.vertId[0]],
offsetAB + matrixAB * convexB.m_vertices[facetB.vertId[1]],
offsetAB + matrixAB * convexB.m_vertices[facetB.vertId[2]],
};
// 頂点Ａ→面Ｂの最近接点算出
for(int i=0;i<3;i++) {
EpxVector3 s;
epxGetClosestPointTriangle(triangleA[i],triangleB[0],triangleB[1],triangleB[2],matrixAB * facetB.normal,s);
EpxFloat dSqr = lengthSqr(triangleA[i]-s);
if(dSqr < closestMinSqr) {
closestMinSqr = dSqr;
closestPointA = triangleA[i];
closestPointB = s;
}
}
// 頂点Ｂ→面Ａの最近接点算出
for(int i=0;i<3;i++) {
EpxVector3 s;
epxGetClosestPointTriangle(triangleB[i],triangleA[0],triangleA[1],triangleA[2],facetA.normal,s);
EpxFloat dSqr = lengthSqr(triangleB[i]-s);
if(dSqr < closestMinSqr) {
closestMinSqr = dSqr;
closestPointA = s;
closestPointB = triangleB[i];
}
}
}
}
}
//epxPrintf("intersection check count %d\n",collCount);
normal = transformA.getUpper3x3() * axisMin;
penetrationDepth = distanceMin;
contactPointA = closestPointA - separation;
contactPointB = offsetBA + matrixBA * closestPointB;
return true;
}
void sleepOrWakeup()
{
PfxFloat sleepVelSqr = sleepVelocity * sleepVelocity;
for(PfxUInt32 i=0;i<(PfxUInt32)numRigidBodies;i++) {
PfxRigidState &state = states[i];
if(SCE_PFX_MOTION_MASK_CAN_SLEEP(state.getMotionType())) {
PfxFloat linVelSqr = lengthSqr(state.getLinearVelocity());
PfxFloat angVelSqr = lengthSqr(state.getAngularVelocity());
if(state.isAwake()) {
if( linVelSqr < sleepVelSqr && angVelSqr < sleepVelSqr ) {
state.incrementSleepCount();
}
else {
state.resetSleepCount();
}
}
}
}
if(island) {
for(PfxUInt32 i=0;i<pfxGetNumIslands(island);i++) {
int numActive = 0;
int numSleep = 0;
int numCanSleep = 0;
{
PfxIslandUnit *islandUnit = pfxGetFirstUnitInIsland(island,(PfxUInt32)i);
for(;islandUnit!=NULL;islandUnit=pfxGetNextUnitInIsland(islandUnit)) {
if(!(SCE_PFX_MOTION_MASK_CAN_SLEEP(states[pfxGetUnitId(islandUnit)].getMotionType()))) continue;
PfxRigidState &state = states[pfxGetUnitId(islandUnit)];
if(state.isAsleep()) {
numSleep++;
}
else {
numActive++;
if(state.getSleepCount() > sleepCount) {
numCanSleep++;
}
}
}
}
// Deactivate Island
if(numCanSleep > 0 && numCanSleep == numActive + numSleep) {
PfxIslandUnit *islandUnit = pfxGetFirstUnitInIsland(island,(PfxUInt32)i);
for(;islandUnit!=NULL;islandUnit=pfxGetNextUnitInIsland(islandUnit)) {
if(!(SCE_PFX_MOTION_MASK_CAN_SLEEP(states[pfxGetUnitId(islandUnit)].getMotionType()))) continue;
states[pfxGetUnitId(islandUnit)].sleep();
}
}
// Activate Island
else if(numSleep > 0 && numActive > 0) {
PfxIslandUnit *islandUnit = pfxGetFirstUnitInIsland(island,(PfxUInt32)i);
for(;islandUnit!=NULL;islandUnit=pfxGetNextUnitInIsland(islandUnit)) {
if(!(SCE_PFX_MOTION_MASK_CAN_SLEEP(states[pfxGetUnitId(islandUnit)].getMotionType()))) continue;
states[pfxGetUnitId(islandUnit)].wakeup();
}
}
}
}
}
void akMeshLoader::convert(bool sortByMat, bool openglVertexColor)
{
Blender::MFace* mface = m_bmesh->mface;
Blender::MVert* mvert = m_bmesh->mvert;
Blender::MCol* mcol = 0;
Blender::MTFace* mtface[8] = {0, 0, 0, 0, 0, 0, 0, 0};
if (!mface || !mvert)
return;
Blender::MVert vpak[4];
unsigned int cpak[4];
unsigned int ipak[4];
int totlayer;
akSubMesh* curSubMesh = 0;
utArray<akSubMeshPair*> meshtable;
utArray<utString> vgroups;
utArray<utString> shapekeys;
utArray<btAlignedObjectArray<akVector3> > shapekeysnormals;
utArray<utArray<UTuint32> > smoothfacesarray;
akMeshLoaderUtils_getLayers(m_bmesh, mtface, &mcol, totlayer);
akMeshLoaderUtils_getVertexGroups(m_bmesh, m_bobj, vgroups);
akMeshLoaderUtils_getShapeKeys(m_bmesh, shapekeys);
if(shapekeys.size()>0)
{
akMeshLoaderUtils_getShapeKeysNormals(m_bmesh, shapekeys.size(), shapekeysnormals);
akMeshLoaderUtils_getSmoothFaces(m_bmesh, smoothfacesarray);
}
for (int fi = 0; fi < m_bmesh->totface; fi++)
{
const Blender::MFace& curface = mface[fi];
// skip if face is not a triangle || quad
if (!curface.v3)
continue;
const bool isQuad = curface.v4 != 0;
TempFace t[2];
PackedFace f;
f.totlay = totlayer;
if (isQuad)
{
vpak[0] = mvert[curface.v1];
vpak[1] = mvert[curface.v2];
vpak[2] = mvert[curface.v3];
vpak[3] = mvert[curface.v4];
ipak[0] = curface.v1;
ipak[1] = curface.v2;
ipak[2] = curface.v3;
ipak[3] = curface.v4;
if (mcol != 0)
{
cpak[0] = packColour(mcol[0], openglVertexColor);
cpak[1] = packColour(mcol[1], openglVertexColor);
cpak[2] = packColour(mcol[2], openglVertexColor);
cpak[3] = packColour(mcol[3], openglVertexColor);
}
else
cpak[0] = cpak[1] = cpak[2] = cpak[3] = 0xFFFFFFFF;
for (int i = 0; i < totlayer; i++)
{
if (mtface[i] != 0)
{
VEC2CPY(f.uvLayers[i][0], mtface[i][fi].uv[0]);
VEC2CPY(f.uvLayers[i][1], mtface[i][fi].uv[1]);
VEC2CPY(f.uvLayers[i][2], mtface[i][fi].uv[2]);
VEC2CPY(f.uvLayers[i][3], mtface[i][fi].uv[3]);
}
}
f.verts = vpak;
f.index = ipak;
f.colors = cpak;
akVector3 e0, e1;
akVector3 v0,v1,v2,v3;
VEC3CPY(v0, mvert[curface.v1].co);
VEC3CPY(v1, mvert[curface.v2].co);
VEC3CPY(v2, mvert[curface.v3].co);
VEC3CPY(v3, mvert[curface.v4].co);
e0 = v0 - v2;
e1 = v1 - v3;
if (lengthSqr(e0) <lengthSqr(e1))
{
convertIndexedTriangle(&t[0], 0, 1, 2, f);
convertIndexedTriangle(&t[1], 2, 3, 0, f);
}
else
{
convertIndexedTriangle(&t[0], 0, 1, 3, f);
convertIndexedTriangle(&t[1], 3, 1, 2, f);
}
}
else
{
for (int i = 0; i < totlayer; i++)
{
if (mtface[i] != 0)
{
VEC2CPY(f.uvLayers[i][0], mtface[i][fi].uv[0]);
VEC2CPY(f.uvLayers[i][1], mtface[i][fi].uv[1]);
VEC2CPY(f.uvLayers[i][2], mtface[i][fi].uv[2]);
}
}
vpak[0] = mvert[curface.v1];
vpak[1] = mvert[curface.v2];
vpak[2] = mvert[curface.v3];
ipak[0] = curface.v1;
ipak[1] = curface.v2;
ipak[2] = curface.v3;
if (mcol != 0)
{
cpak[0] = packColour(mcol[0], openglVertexColor);
cpak[1] = packColour(mcol[1], openglVertexColor);
cpak[2] = packColour(mcol[2], openglVertexColor);
}
else
cpak[0] = cpak[1] = cpak[2] = cpak[3] = 0xFFFFFFFF;
f.verts = vpak;
f.index = ipak;
f.colors = cpak;
convertIndexedTriangle(&t[0], 0, 1, 2, f);
}
akSubMeshHashKey test;
if (sortByMat)
{
int mode = 0;
if (mtface[0])
mode = mtface[0][fi].mode;
test = akSubMeshHashKey(curface.mat_nr, mode);
}
else
{
Blender::Image* ima[8] = {0, 0, 0, 0, 0, 0, 0, 0};
for (int i = 0; i < totlayer; i++)
{
if (mtface[i] != 0)
ima[i] = mtface[i][fi].tpage;
}
int mode = 0, alpha = 0;
if (mtface[0])
{
mode = mtface[0][fi].mode;
alpha = mtface[0][fi].transp;
}
test = akSubMeshHashKey(mode, alpha, ima);
}
// find submesh
UTsize arpos = 0;
akSubMeshPair* curSubMeshPair = 0;
for(arpos=0; arpos<meshtable.size(); arpos++)
{
if(meshtable[arpos]->test == test)
break;
}
if (arpos >= meshtable.size())
{
curSubMesh = new akSubMesh(akSubMesh::ME_TRIANGLES, true, true, totlayer);
m_gmesh->addSubMesh(curSubMesh);
curSubMeshPair = new akSubMeshPair(curSubMesh, m_bmesh, &smoothfacesarray, &shapekeysnormals);
curSubMeshPair->test = test;
for(int i=0; i<vgroups.size(); i++)
{
akVertexGroup* vg = new akVertexGroup();
vg->setName(vgroups[i]);
curSubMesh->addVertexGroup(vg);
}
for(int i=0; i<shapekeys.size(); i++)
{
akMorphTarget* mt = new akMorphTarget(true);
mt->setName(shapekeys[i]);
curSubMesh->addMorphTarget(mt);
}
meshtable.push_back(curSubMeshPair);
}
else
{
curSubMeshPair = meshtable.at(arpos);
curSubMesh = curSubMeshPair->item;
}
if (curSubMesh == 0 || curSubMeshPair == 0) continue;
if (!(curface.flag & ME_SMOOTH))
{
// face normal
calcNormal(&t[0]);
if (isQuad)
{
calcNormal(&t[1]);
akVector3 n = normalize(t[0].v0.no + t[1].v0.no);
t[0].v0.no = t[0].v1.no = t[0].v2.no = t[1].v0.no = t[1].v1.no = t[1].v2.no = n;
}
}
addTriangle(curSubMeshPair, fi, t[0].v0, t[0].i0,
t[0].v1, t[0].i1,
t[0].v2, t[0].i2);
if (isQuad)
{
addTriangle(curSubMeshPair, fi, t[1].v0, t[1].i0,
t[1].v1, t[1].i1,
t[1].v2, t[1].i2);
}
if (mcol)
mcol += 4;
}
utArrayIterator<utArray<akSubMeshPair*> > iter(meshtable);
while (iter.hasMoreElements())
{
akSubMeshPair* subpair = iter.peekNext();
if(subpair->test.m_blenderMat)
{
Blender::Material* bmat = akMeshLoaderUtils_getMaterial(m_bobj, subpair->test.m_matnr);
if (bmat)
convertMaterial(bmat, subpair);
}
else
convertTextureFace(subpair);
delete subpair;
iter.getNext();
}
meshtable.clear();
for(int i=0; i<vgroups.size(); i++)
{
vgroups[i].clear();
}
vgroups.clear();
for(int i=0; i<shapekeys.size(); i++)
{
shapekeys[i].clear();
}
shapekeys.clear();
for(int i=0; i<smoothfacesarray.size(); i++)
{
smoothfacesarray[i].clear();
}
smoothfacesarray.clear();
for(int i=0; i<shapekeysnormals.size(); i++)
{
shapekeysnormals[i].clear();
}
shapekeysnormals.clear();
}
T length() const {return sqrt(lengthSqr());}
const bool operator<(const vec2 &l, const vec2 &r) { return lengthSqr(l) > lengthSqr(r); }
PfxInt32 pfxCreateLargeTriMesh(PfxLargeTriMesh &lmesh,const PfxCreateLargeTriMeshParam ¶m)
{
// Check input
if(param.numVerts == 0 || param.numTriangles == 0 || !param.verts || !param.triangles)
return SCE_PFX_ERR_INVALID_VALUE;
if(param.islandsRatio < 0.0f || param.islandsRatio > 1.0f)
return SCE_PFX_ERR_OUT_OF_RANGE;
if(param.numFacetsLimit == 0 || param.numFacetsLimit > SCE_PFX_NUMMESHFACETS)
return SCE_PFX_ERR_OUT_OF_RANGE;
const PfxFloat epsilon = 0.00001f;
PfxArray<PfxMcVert> vertList(param.numVerts); // 頂点配列
PfxArray<PfxMcFacet> facetList(param.numTriangles); // 面配列
PfxArray<PfxMcEdge> edgeList(param.numTriangles*3); // エッジ配列
PfxArray<PfxMcEdge*> edgeHead(param.numTriangles*3);
//J 頂点配列作成
for(PfxUInt32 i=0;i<param.numVerts;i++) {
PfxFloat *vtx = (PfxFloat*)((uintptr_t)param.verts + param.vertexStrideBytes * i);
PfxMcVert mcv;
mcv.flag = 0;
mcv.i = i;
mcv.coord = pfxReadVector3(vtx);
vertList.push(mcv);
}
// 面配列作成
for(PfxUInt32 i=0;i<param.numTriangles;i++) {
void *ids = (void*)((uintptr_t)param.triangles + param.triangleStrideBytes * i);
PfxUInt32 idx[3];
if(param.flag & SCE_PFX_MESH_FLAG_32BIT_INDEX) {
if(param.flag & SCE_PFX_MESH_FLAG_NORMAL_FLIP) {
idx[0] = ((PfxUInt32*)ids)[2];
idx[1] = ((PfxUInt32*)ids)[1];
idx[2] = ((PfxUInt32*)ids)[0];
}
else {
idx[0] = ((PfxUInt32*)ids)[0];
idx[1] = ((PfxUInt32*)ids)[1];
idx[2] = ((PfxUInt32*)ids)[2];
}
}
else if(param.flag & SCE_PFX_MESH_FLAG_16BIT_INDEX) {
if(param.flag & SCE_PFX_MESH_FLAG_NORMAL_FLIP) {
idx[0] = ((PfxUInt16*)ids)[2];
idx[1] = ((PfxUInt16*)ids)[1];
idx[2] = ((PfxUInt16*)ids)[0];
}
else {
idx[0] = ((PfxUInt16*)ids)[0];
idx[1] = ((PfxUInt16*)ids)[1];
idx[2] = ((PfxUInt16*)ids)[2];
}
}
else {
return SCE_PFX_ERR_INVALID_FLAG;
}
const PfxVector3 pnts[3] = {
vertList[idx[0]].coord,
vertList[idx[1]].coord,
vertList[idx[2]].coord,
};
// 面積が０の面を排除
PfxFloat area = lengthSqr(cross(pnts[1]-pnts[0],pnts[2]-pnts[0]));
if((param.flag & SCE_PFX_MESH_FLAG_AUTO_ELIMINATION) && area < 0.00001f) {
continue;
}
PfxMcFacet facet;
facet.v[0] = &vertList[idx[0]];
facet.v[1] = &vertList[idx[1]];
facet.v[2] = &vertList[idx[2]];
facet.e[0] = facet.e[1] = facet.e[2] = NULL;
facet.n = normalize(cross(pnts[2]-pnts[1],pnts[0]-pnts[1]));
facet.area = area;
facet.thickness = param.defaultThickness;
facet.neighbor[0] = facet.neighbor[1] = facet.neighbor[2] = -1;
facet.neighborEdgeId[0] = facet.neighborEdgeId[1] = facet.neighborEdgeId[2] = -1;
facetList.push(facet);
}
const PfxUInt32 numTriangles = facetList.size();
{
PfxArray<PfxMcTriList> triEntry(numTriangles*3);
PfxArray<PfxMcTriList*> triHead(numTriangles*3); // 頂点から面への参照リスト
PfxInt32 cnt = 0;
PfxMcTriList* nl = NULL;
triEntry.assign(numTriangles*3,PfxMcTriList());
triHead.assign(numTriangles*3,nl);
// 頂点から面への参照リストを作成
for(PfxUInt32 i=0;i<numTriangles;i++) {
for(PfxUInt32 v=0;v<3;v++) {
PfxUInt32 vertId = facetList[i].v[v]->i;
triEntry[cnt].facet = &facetList[i];
triEntry[cnt].next = triHead[vertId];
triHead[vertId] = &triEntry[cnt++];
}
}
// 同一頂点をまとめる
if(param.flag & SCE_PFX_MESH_FLAG_AUTO_ELIMINATION) {
for(PfxUInt32 i=0;i<param.numVerts;i++) {
if(vertList[i].flag == 1) continue;
for(PfxUInt32 j=i+1;j<param.numVerts;j++) {
if(vertList[j].flag == 1) continue;
PfxFloat lenSqr = lengthSqr(vertList[i].coord-vertList[j].coord);
if(lenSqr < epsilon) {
//SCE_PFX_PRINTF("same position %d,%d\n",i,j);
vertList[j].flag = 1; // 同一点なのでフラグを立てる
for(PfxMcTriList *f=triHead[j];f!=NULL;f=f->next) {
for(PfxInt32 k=0;k<3;k++) {
if(f->facet->v[k] == &vertList[j]) {
f->facet->v[k] = &vertList[i]; // 頂点を付け替える
break;
}
}
}
}
}
}
}
}
// 接続面間の角度を算出して面にセット
PfxMcEdge *nl = NULL;
edgeHead.assign(numTriangles*3,nl);
edgeList.assign(numTriangles*3,PfxMcEdge());
// エッジ配列の作成
PfxUInt32 ecnt = 0;
for(PfxUInt32 i=0;i<numTriangles;i++) {
PfxMcFacet &f = facetList[i];
for(PfxUInt32 v=0;v<3;v++) {
uintptr_t vp1 = ((uintptr_t)f.v[v]-(uintptr_t)&vertList[0])/sizeof(PfxMcVert);
uintptr_t vp2 = ((uintptr_t)f.v[(v+1)%3]-(uintptr_t)&vertList[0])/sizeof(PfxMcVert);
PfxUInt32 viMin = SCE_PFX_MIN(vp1,vp2);
PfxUInt32 viMax = SCE_PFX_MAX(vp1,vp2);
PfxInt32 key = ((0x8da6b343*viMin+0xd8163841*viMax)%(numTriangles*3));
for(PfxMcEdge *e = edgeHead[key];;e=e->next) {
if(!e) {
edgeList[ecnt].vertId[0] = viMin;
edgeList[ecnt].vertId[1] = viMax;
edgeList[ecnt].facetId[0] = i;
edgeList[ecnt].edgeId[0] = v;
edgeList[ecnt].numFacets = 1;
edgeList[ecnt].next = edgeHead[key];
edgeList[ecnt].angleType = SCE_PFX_EDGE_CONVEX;
edgeList[ecnt].angle = 0.0f;
edgeHead[key] = &edgeList[ecnt];
f.e[v] = &edgeList[ecnt];
ecnt++;
break;
}
if(e->vertId[0] == viMin && e->vertId[1] == viMax) {
SCE_PFX_ALWAYS_ASSERT_MSG(e->numFacets == 1,"An edge connected with over 2 triangles is invalid");
e->facetId[1] = i;
e->edgeId[1] = v;
e->numFacets = 2;
f.e[v] = e;
f.neighbor[v] = e->facetId[0];
f.neighborEdgeId[v] = e->edgeId[0];
facetList[e->facetId[0]].neighbor[e->edgeId[0]] = i;
facetList[e->facetId[0]].neighborEdgeId[e->edgeId[0]] = e->edgeId[1];
break;
}
}
}
}
// 角度を計算
for(PfxUInt32 i=0;i<numTriangles;i++) {
PfxMcFacet &facetA = facetList[i];
PfxQueue<PfxMcFacetLink> cqueue(ecnt);
for(PfxUInt32 j=0;j<3;j++) {
if(facetA.neighbor[j] >= 0) {
cqueue.push(PfxMcFacetLink(
j,
facetA.e[j]->vertId[0],facetA.e[j]->vertId[1],
i,j,
facetA.neighbor[j],facetA.neighborEdgeId[j]));
}
}
while(!cqueue.empty()) {
PfxMcFacetLink link = cqueue.front();
cqueue.pop();
PfxMcFacet &ofacet = facetList[link.ofacetId];
PfxMcEdge *edge = ofacet.e[link.oedgeId];
// facetAとのなす角を計算
{
// 面に含まれるが、このエッジに含まれない点
PfxUInt32 ids[3] = {2,0,1};
PfxVector3 v1 = facetA.v[ids[link.baseEdgeId]]->coord;
PfxVector3 v2 = ofacet.v[ids[link.oedgeId]]->coord;
// エッジの凹凸判定
PfxVector3 midPnt = (v1 + v2) * 0.5f;
PfxVector3 pntOnEdge = facetA.v[link.baseEdgeId]->coord;
PfxFloat chk1 = dot(facetA.n,midPnt-pntOnEdge);
PfxFloat chk2 = dot(ofacet.n,midPnt-pntOnEdge);
if(chk1 < -epsilon && chk2 < -epsilon) {
if(link.ifacetId == i) edge->angleType = SCE_PFX_EDGE_CONVEX;
// 厚み角の判定に使う角度をセット
if(param.flag & SCE_PFX_MESH_FLAG_AUTO_THICKNESS) {
edge->angle = 0.5f*acosf(dot(facetA.n,ofacet.n));
}
}
else if(chk1 > epsilon && chk2 > epsilon) {
if(link.ifacetId == i) edge->angleType = SCE_PFX_EDGE_CONCAVE;
}
else {
if(link.ifacetId == i) edge->angleType = SCE_PFX_EDGE_FLAT;
}
}
// 次の接続面を登録（コメントアウトすると頂点で接続された面を考慮しない）
if(param.flag & SCE_PFX_MESH_FLAG_AUTO_THICKNESS) {
PfxInt32 nextEdgeId = (link.oedgeId+1)%3;
PfxMcEdge *nextEdge = ofacet.e[nextEdgeId];
if(ofacet.neighbor[nextEdgeId] >= 0 && ofacet.neighbor[nextEdgeId] != i &&
((PfxInt32)nextEdge->vertId[0] == link.vid1 || (PfxInt32)nextEdge->vertId[0] == link.vid2 ||
(PfxInt32)nextEdge->vertId[1] == link.vid1 || (PfxInt32)nextEdge->vertId[1] == link.vid2) ) {
cqueue.push(PfxMcFacetLink(
link.baseEdgeId,
link.vid1,link.vid2,
link.ofacetId,link.iedgeId,
ofacet.neighbor[nextEdgeId],ofacet.neighborEdgeId[nextEdgeId]));
}
nextEdgeId = (link.oedgeId+2)%3;
nextEdge = ofacet.e[nextEdgeId];
if(ofacet.neighbor[nextEdgeId] >= 0 && ofacet.neighbor[nextEdgeId] != i &&
((PfxInt32)nextEdge->vertId[0] == link.vid1 || (PfxInt32)nextEdge->vertId[0] == link.vid2 ||
(PfxInt32)nextEdge->vertId[1] == link.vid1 || (PfxInt32)nextEdge->vertId[1] == link.vid2) ) {
cqueue.push(PfxMcFacetLink(
link.baseEdgeId,
link.vid1,link.vid2,
link.ofacetId,link.iedgeId,
ofacet.neighbor[nextEdgeId],ofacet.neighborEdgeId[nextEdgeId]));
}
}
}
}
// 面に厚みを付ける
if(param.flag & SCE_PFX_MESH_FLAG_AUTO_THICKNESS) {
for(PfxUInt32 i=0;i<numTriangles;i++) {
PfxMcFacet &facetA = facetList[i];
for(PfxUInt32 j=0;j<numTriangles;j++) {
// 隣接面は比較対象にしない
if( i==j ||
j == (PfxInt32)facetA.e[0]->facetId[0] ||
j == (PfxInt32)facetA.e[0]->facetId[1] ||
j == (PfxInt32)facetA.e[1]->facetId[0] ||
j == (PfxInt32)facetA.e[1]->facetId[1] ||
j == (PfxInt32)facetA.e[2]->facetId[0] ||
j == (PfxInt32)facetA.e[2]->facetId[1]) {
continue;
}
PfxMcFacet &facetB = facetList[j];
// 交差判定
PfxFloat closestDistance=0;
if(intersect(facetA,facetB,closestDistance)) {
// 最近接距離/2を厚みとして採用
facetA.thickness = SCE_PFX_MAX(param.defaultThickness,SCE_PFX_MIN(facetA.thickness,closestDistance * 0.5f));
}
}
}
}
// 面の面積によって３種類に分類する
PfxFloat areaMin=SCE_PFX_FLT_MAX,areaMax=-SCE_PFX_FLT_MAX;
for(PfxUInt32 f=0;f<(PfxUInt32)numTriangles;f++) {
PfxVector3 pnts[3] = {
facetList[f].v[0]->coord,
facetList[f].v[1]->coord,
facetList[f].v[2]->coord,
};
areaMin = SCE_PFX_MIN(areaMin,facetList[f].area);
areaMax = SCE_PFX_MAX(areaMax,facetList[f].area);
// 面のAABBを算出
facetList[f].aabbMin = minPerElem(pnts[2],minPerElem(pnts[1],pnts[0]));
facetList[f].aabbMax = maxPerElem(pnts[2],maxPerElem(pnts[1],pnts[0]));
}
PfxFloat areaDiff = (areaMax-areaMin)/3.0f;
PfxFloat areaLevel0,areaLevel1;
areaLevel0 = areaMin + areaDiff;
areaLevel1 = areaMin + areaDiff * 2.0f;
PfxArray<PfxMcFacetPtr> facetsLv0(numTriangles);
PfxArray<PfxMcFacetPtr> facetsLv1(numTriangles);
PfxArray<PfxMcFacetPtr> facetsLv2(numTriangles);
for(PfxUInt32 f=0;f<numTriangles;f++) {
PfxFloat area = facetList[f].area;
PfxMcFacet *fct = &facetList[f];
if(area < areaLevel0) {
facetsLv0.push(fct);
}
else if(area > areaLevel1) {
facetsLv2.push(fct);
}
else {
facetsLv1.push(fct);
}
}
// アイランドの配列
PfxMcIslands islands;
PfxVector3 lmeshSize;
// レベル毎にPfxTriMeshを作成
if(!facetsLv0.empty()) {
// 全体のAABBを求める
PfxVector3 aabbMin,aabbMax,center,half;
aabbMin =facetsLv0[0]->aabbMin;
aabbMax = facetsLv0[0]->aabbMax;
for(PfxUInt32 f=1;f<facetsLv0.size();f++) {
aabbMin = minPerElem(facetsLv0[f]->aabbMin,aabbMin);
aabbMax = maxPerElem(facetsLv0[f]->aabbMax,aabbMax);
}
center = ( aabbMin + aabbMax ) * 0.5f;
half = ( aabbMax - aabbMin ) * 0.5f;
// 再帰的に処理
divideMeshes(
param.numFacetsLimit,param.islandsRatio,
islands,
facetsLv0,
center,half);
lmeshSize = maxPerElem(lmeshSize,maxPerElem(absPerElem(aabbMin),absPerElem(aabbMax)));
}
if(!facetsLv1.empty()) {
// 全体のAABBを求める
PfxVector3 aabbMin,aabbMax,center,half;
aabbMin =facetsLv1[0]->aabbMin;
aabbMax = facetsLv1[0]->aabbMax;
for(PfxUInt32 f=1;f<facetsLv1.size();f++) {
aabbMin = minPerElem(facetsLv1[f]->aabbMin,aabbMin);
aabbMax = maxPerElem(facetsLv1[f]->aabbMax,aabbMax);
}
center = ( aabbMin + aabbMax ) * 0.5f;
half = ( aabbMax - aabbMin ) * 0.5f;
// 再帰的に処理
divideMeshes(
param.numFacetsLimit,param.islandsRatio,
islands,
facetsLv1,
center,half);
lmeshSize = maxPerElem(lmeshSize,maxPerElem(absPerElem(aabbMin),absPerElem(aabbMax)));
}
if(!facetsLv2.empty()) {
// 全体のAABBを求める
PfxVector3 aabbMin,aabbMax,center,half;
aabbMin =facetsLv2[0]->aabbMin;
aabbMax = facetsLv2[0]->aabbMax;
for(PfxUInt32 f=1;f<facetsLv2.size();f++) {
aabbMin = minPerElem(facetsLv2[f]->aabbMin,aabbMin);
aabbMax = maxPerElem(facetsLv2[f]->aabbMax,aabbMax);
}
center = ( aabbMin + aabbMax ) * 0.5f;
half = ( aabbMax - aabbMin ) * 0.5f;
// 再帰的に処理
divideMeshes(
param.numFacetsLimit,param.islandsRatio,
islands,
facetsLv2,
center,half);
lmeshSize = maxPerElem(lmeshSize,maxPerElem(absPerElem(aabbMin),absPerElem(aabbMax)));
}
lmesh.m_half = lmeshSize;
// Check Islands
//for(PfxInt32 i=0;i<islands.numIslands;i++) {
// SCE_PFX_PRINTF("island %d\n",i);
// for(PfxInt32 f=0;f<islands.facetsInIsland[i].size();f++) {
// PfxMcFacet *facet = islands.facetsInIsland[i][f];
// SCE_PFX_PRINTF(" %d %d %d\n",facet->v[0]->i,facet->v[1]->i,facet->v[2]->i);
// }
//}
// PfxLargeTriMeshの生成
if(islands.numIslands > 0) {
lmesh.m_numIslands = 0;
lmesh.m_aabbList = (PfxAabb16*)SCE_PFX_UTIL_ALLOC(128,sizeof(PfxAabb16)*islands.numIslands);
lmesh.m_islands = (PfxTriMesh*)SCE_PFX_UTIL_ALLOC(128,sizeof(PfxTriMesh)*islands.numIslands);
PfxInt32 maxFacets=0,maxVerts=0,maxEdges=0;
for(PfxUInt32 i=0;i<islands.numIslands;i++) {
PfxTriMesh island;
createIsland(island,islands.facetsInIsland[i]);
addIslandToLargeTriMesh(lmesh,island);
maxFacets = SCE_PFX_MAX(maxFacets,island.m_numFacets);
maxVerts = SCE_PFX_MAX(maxVerts,island.m_numVerts);
maxEdges = SCE_PFX_MAX(maxEdges,island.m_numEdges);
//SCE_PFX_PRINTF("island %d verts %d edges %d facets %d\n",i,island.m_numVerts,island.m_numEdges,island.m_numFacets);
}
SCE_PFX_PRINTF("generate completed!\n\tinput mesh verts %d triangles %d\n\tislands %d max triangles %d verts %d edges %d\n",
param.numVerts,param.numTriangles,
lmesh.m_numIslands,maxFacets,maxVerts,maxEdges);
SCE_PFX_PRINTF("\tsizeof(PfxLargeTriMesh) %d sizeof(PfxTriMesh) %d\n",sizeof(PfxLargeTriMesh),sizeof(PfxTriMesh));
}
else {
SCE_PFX_PRINTF("islands overflow! %d/%d\n",islands.numIslands,SCE_PFX_LARGETRIMESH_MAX_ISLANDS);
return SCE_PFX_ERR_OUT_OF_RANGE;
}
return SCE_PFX_OK;
}
const float length(const vec2 &v)
{
return sqrt(lengthSqr(v));
};
