RomkaZVO/FC1
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
Files
main
FC1/CryPhysics/cylindergeom.cpp
romkazvo 34d6c5d489 123
2023-08-07 19:29:24 +08:00

617 lines
20 KiB
C++
Raw Permalink Blame History

#include "stdafx.h"
#include "utils.h"
#include "primitives.h"
#include "unprojectionchecks.h"
#include "bvtree.h"
#include "singleboxtree.h"
#include "geometry.h"
#include "cylindergeom.h"
#include "trimesh.h"
#include "boxgeom.h"
vector2df g_CylCont[64];
int g_CylContId[64];
CCylinderGeom* CCylinderGeom::CreateCylinder(cylinder *pcyl)
{
m_cyl.center = pcyl->center;
m_cyl.axis = pcyl->axis;
m_cyl.hh = pcyl->hh;
m_cyl.r = pcyl->r;
box bbox;
bbox.Basis.SetRow(2,m_cyl.axis);
//bbox.Basis.SetRow(0,m_cyl.axis.orthogonal().normalized());
bbox.Basis.SetRow(0,GetOrthogonal(m_cyl.axis).normalized());
bbox.Basis.SetRow(1,m_cyl.axis ^ bbox.Basis.GetRow(0));
bbox.bOriented = 1;
bbox.center = m_cyl.center;
bbox.size.z = m_cyl.hh;
bbox.size.x = bbox.size.y = m_cyl.r;
m_Tree.SetBox(&bbox);
m_Tree.Build(this);
m_minVtxDist = (m_cyl.r+m_cyl.hh)*1E-4f;
return this;
}
int CCylinderGeom::CalcPhysicalProperties(phys_geometry *pgeom)
{
pgeom->pGeom = this;
pgeom->origin = m_cyl.center;
//pgeom->q = quaternionf(vectorf(0,0,1),m_cyl.axis);
pgeom->q.SetRotationV0V1(vectorf(0,0,1),m_cyl.axis);
pgeom->V = sqr(m_cyl.r)*m_cyl.hh*(pi*2);
float x2=pi*m_cyl.hh*sqr(sqr(m_cyl.r))*(1.0/2), z2=pi*sqr(m_cyl.r)*cube(m_cyl.hh)*(2.0/3);
pgeom->Ibody.Set(x2+z2,x2+z2,x2*2);
return 1;
}
int CCylinderGeom::PointInsideStatus(const vectorf &pt)
{
vectorf ptr; float h;
ptr = pt-m_cyl.center; h = m_cyl.axis*ptr; ptr -= m_cyl.axis*h;
return isneg(ptr.len2()-sqr(m_cyl.r)) & isneg(fabs_tpl(h)-m_cyl.hh);
}
int CCylinderGeom::PrepareForIntersectionTest(geometry_under_test *pGTest, CGeometry *pCollider,geometry_under_test *pGTestColl, bool bKeepPrevContacts)
{
static short g_CylIdBuf[1];
static surface_desc g_CylSurfaceBuf[1];
static edge_desc g_CylEdgeBuf[1];
pGTest->pGeometry = this;
pGTest->pBVtree = &m_Tree;
m_Tree.PrepareForIntersectionTest(pGTest);
pGTest->primbuf = pGTest->primbuf1 = g_CylBuf+g_CylBufPos++;
pGTest->szprimbuf = 1;
pGTest->typeprim = cylinder::type;
pGTest->szprim = sizeof(cylinder);
pGTest->idbuf = g_CylIdBuf;
pGTest->surfaces = g_CylSurfaceBuf;
pGTest->edges = g_CylEdgeBuf;
pGTest->minAreaEdge = pi*m_cyl.r*2.0f/m_nTesselation;
return 1;
}
void CCylinderGeom::PrepareCylinder(cylinder *pcyl, geometry_under_test *pGTest)
{
pcyl->center = pGTest->R*m_cyl.center*pGTest->scale + pGTest->offset;
pcyl->axis = pGTest->R*m_cyl.axis;
pcyl->hh = m_cyl.hh*pGTest->scale;
pcyl->r = m_cyl.r*pGTest->scale;
}
int CCylinderGeom::PreparePrimitive(geom_world_data *pgwd, primitive *&pprim)
{
cylinder *pcyl = g_CylBuf+g_CylBufPos;
g_CylBufPos = g_CylBufPos+1 & sizeof(g_CylBuf)/sizeof(g_CylBuf[0])-1;
pcyl->center = pgwd->R*m_cyl.center*pgwd->scale + pgwd->offset;
pcyl->axis = pgwd->R*m_cyl.axis;
pcyl->hh = m_cyl.hh*pgwd->scale;
pcyl->r = m_cyl.r*pgwd->scale;
pprim = pcyl;
return cylinder::type;
}
int CCylinderGeom::GetPrimitiveList(int iStart,int nPrims, int typeCollider,primitive *pCollider,int bColliderLocal,
geometry_under_test *pGTest,geometry_under_test *pGTestOp, primitive *pRes,short *pResId)
{
PrepareCylinder((cylinder*)pRes, pGTest);
pGTest->bTransformUpdated = 0; *pResId = -1;
return 1;
}
int CCylinderGeom::GetUnprojectionCandidates(int iop,const contact *pcontact, primitive *&pprim,int *&piFeature, geometry_under_test *pGTest)
{
if (pGTest->bTransformUpdated) {
pprim = pGTest->primbuf1;
PrepareCylinder((cylinder*)pprim, pGTest);
}
cylinder *pcyl = (cylinder*)pprim;
pGTest->idbuf[0] = -1;
int iFeature = pcontact->iFeature[iop],iCap;
float r2,hh;
r2 = (pcontact->pt-pcyl->center^pcyl->axis).len2();
hh = (pcontact->pt-pcyl->center)*pcyl->axis;
if ((iFeature&0xE0)!=0x20 && fabs_tpl(r2-sqr(pcyl->r))<sqr(m_minVtxDist) && fabs_tpl(fabs_tpl(hh)-pcyl->hh)<m_minVtxDist)
iFeature = 0x20 | isnonneg(hh);
if ((iFeature&0xE0)==0x20 || iFeature>0x40) {
iCap = (iFeature&3)-(iFeature>>6);
pGTest->surfaces[0].n = pcyl->axis*((iCap<<1)-1);
pGTest->surfaces[0].idx = 0;
pGTest->surfaces[0].iFeature = 0x40 | iCap+1;
pGTest->nSurfaces = 1;
} else
pGTest->nSurfaces = 0;
if ((iFeature&0xE0)==0x20 || iFeature==0x40) {
pGTest->edges[0].dir = pcyl->axis;
pGTest->edges[0].n[0] = pcyl->center-pcontact->pt^pcyl->axis;
pGTest->edges[0].n[1] = -pGTest->edges[0].n[0];
pGTest->edges[0].idx = 0;
pGTest->edges[0].iFeature = iFeature;
pGTest->nEdges = 1;
} else
pGTest->nEdges = 0;
return 1;
}
int CCylinderGeom::PreparePolygon(coord_plane *psurface, int iPrim,int iFeature, geometry_under_test *pGTest, vector2df *&ptbuf,
int *&pVtxIdBuf,int *&pEdgeIdBuf)
{
int i,icap;
float ang,step;
vectorf axes[3],pt,center;
axes[2] = pGTest->R*m_cyl.axis;
//axes[0] = axes[2].orthogonal().normalized();
axes[0] = GetOrthogonal(axes[2]).normalized();
axes[1] = axes[2]^axes[0];
icap = (((iFeature&3)-(iFeature>>6)<<1)-1);
center = pGTest->R*m_cyl.center*pGTest->scale + axes[2]*(m_cyl.hh*pGTest->scale*icap) + pGTest->offset;
if (psurface->n.len2()==0) {
psurface->n = axes[2]*icap;
psurface->axes[0] = axes[0]; psurface->axes[1] = axes[1]*icap;
psurface->origin = center;
}
center -= psurface->origin;
step = (pi*2/m_nTesselation)*sgnnz(axes[2]*psurface->n);
axes[0] *= (pGTest->scale*m_cyl.r);
axes[1] *= (pGTest->scale*m_cyl.r);
for(i=0,ang=0;i<m_nTesselation;i++,ang+=step) {
pt = center + axes[0]*cos_tpl(ang) + axes[1]*sin_tpl(ang);
g_CylCont[i].set(pt*psurface->axes[0],pt*psurface->axes[1]);
g_CylContId[i] = 0x20 | icap+1>>1;
}
g_CylCont[i] = g_CylCont[0]; g_CylContId[2] = g_CylContId[0];
ptbuf = g_CylCont;
pVtxIdBuf = pEdgeIdBuf = g_CylContId;
return i;
}
int CCylinderGeom::PreparePolyline(coord_plane *psurface, int iPrim,int iFeature, geometry_under_test *pGTest, vector2df *&ptbuf,
int *&pVtxIdBuf,int *&pEdgeIdBuf)
{
vectorf axis,center,pt;
axis = pGTest->R*m_cyl.axis;
center = pGTest->R*m_cyl.center*pGTest->scale + pGTest->offset;
pt = center + axis*(m_cyl.hh*pGTest->scale) - psurface->origin;
g_CylCont[0].set(pt*psurface->axes[0],pt*psurface->axes[1]);
pt = center - axis*(m_cyl.hh*pGTest->scale) - psurface->origin;
g_CylCont[1].set(pt*psurface->axes[0],pt*psurface->axes[1]);
g_CylCont[2] = g_CylCont[0];
g_CylContId[0] = g_CylContId[1] = g_CylContId[2] = 0x40;
ptbuf = g_CylCont;
pVtxIdBuf = pEdgeIdBuf = g_CylContId;
return 2;
}
int CCylinderGeom::FindClosestPoint(geom_world_data *pgwd, int &iPrim,int &iFeature, const vectorf &ptdst0,const vectorf &ptdst1,
vectorf *ptres, int nMaxIters)
{
vectorr axis,center,pt,l,ptdst[]={ptdst0,ptdst1},ptresi[2];
real r,hh,r2;
int i,icap,bLine;
axis = pgwd->R*m_cyl.axis;
center = pgwd->R*m_cyl.center*pgwd->scale + pgwd->offset;
r = m_cyl.r*pgwd->scale; r2 = r*r; hh = m_cyl.hh*pgwd->scale;
pt = ptdst0-center; ptres[1] = ptdst0;
if (bLine = isneg(r2*1E-6f-(l=ptdst1-ptdst0).len2())) {
vectorr n,la,pa;
real n2,l2,t0,t1,kt,pl,pal,roots[4];
polynomial_tpl<real,4> pn;
n = l^axis; n2 = n.len2();
if (isneg(n2*r2-sqr(pt*n)) & inrange(t0=(-pt^axis)*n, (real)0,n2) & inrange(t1=(-pt^l)*n, (real)0,n2*hh)) {
ptres[1] = ptdst0+l*(t0/n2); // line-cylinder side distance
ptres[0] = center+axis*(axis*(ptres[1]-center));
ptres[0] = center+(ptres[1]-ptres[0]).normalized()*r;
return 1;
}
icap = sgnnz(t1);
la = l-axis*(l*axis); l2 = l.len2(); kt = l2-sqr(axis*l);
pt = ptdst0-(center+axis*(hh*icap));
pa = pt-axis*(pt*axis); pl = pt*l; pal = pa*l;
pn = psqr(P1(l2)+pt*l)*(P2(la.len2())+P1((pa*la)*2)+pa.len2()) - psqr(P1(kt*r)+(pa*l)*r);
if (pn.nroots(0,1)) for(i=pn.findroots(0,1,roots)-1;i>=0;i--)
if ((pl+l2*roots[i])*(pal+kt*roots[i])>0) { // skips phantom roots
pa = pt+l*roots[i];
if (isneg(r2-(pa^axis).len2()) & isneg((pa*axis)*-icap)) { // check if this is the global minimum
center += axis*(hh*icap);
ptres[1] = center + pa;
ptres[0] = center + (pa-axis*(axis*pa)).normalized()*r;
return 1;
}
}
}
ptresi[1].zero();
for(i=0;i<=bLine;i++) {
pt = ptdst[i]-center;
if (fabsf(pt*axis)<hh) { // the closest point lies on cylinder side
pt -= axis*(axis*pt);
ptresi[i] = ptdst[i]-pt+pt.normalized()*r;
continue;
}
if ((pt^axis).len2()<r*r) { // .. cylinder cap
ptresi[i] = ptdst[i]+axis*(sgn(pt*axis)*hh-pt*axis);
continue;
}
// cylinder cap edge
ptresi[i] = center+axis*(sgn(axis*pt)*hh) + (pt-axis*(axis*pt)).normalized()*r;
}
i = bLine & isneg((ptresi[1]-ptdst[1]).len2()-(ptresi[0]-ptdst[0]).len2());
ptres[0] = ptresi[i];
ptres[1] = ptdst[i];
return 1;
}
int CCylinderGeom::UnprojectSphere(vectorf center,float r,float rsep, contact *pcontact)
{
vectorf dc = center-m_cyl.center;
float axdist2,capdist,r2=sqr(m_cyl.r);
axdist2 = max((dc^m_cyl.axis).len2(),r2);
capdist = max(0.0f,fabsf(dc*m_cyl.axis)-m_cyl.hh);
if (sqr_signed(axdist2+r2+sqr(capdist)-sqr(rsep)) > axdist2*r2*4)
return 0;
unprojection_mode umode;
sphere sph;
sph.center = center;
sph.r = r;
umode.dir.Set(0,0,0);
cylinder_sphere_lin_unprojection(&umode,&m_cyl,-1,&sph,-1,pcontact,0);
pcontact->pt -= umode.dir*pcontact->t;
return 1;
}
void CCylinderGeom::CalcVolumetricPressure(geom_world_data *gwd, const vectorf &epicenter,float k,float rmin,
const vectorf &centerOfMass, vectorf &P,vectorf &L)
{
vectorf c,dc;
float blendf,area,r;
c = gwd->R*m_cyl.center*gwd->scale + gwd->offset;
dc = c-epicenter; r = dc.len();
if (r>m_cyl.r*0.01f)
dc/=r;
else dc.Set(0,0,1);
blendf = (gwd->R*m_cyl.axis^dc).len();
area = (pi*sqr(m_cyl.r)*(1.0f-blendf) + m_cyl.hh*m_cyl.r*4*blendf)*sqr(gwd->scale);
P += dc*(area*k/sqr(max(r,rmin)));
}
// debug testing code
/*float x,x0,dx=r*0.001f,Stest,Stesta,CxStest,CxStesta,Vtest,CxVtest,CzVtest,CxVtesta,CzVtesta;
for(x0=a,Vtest=CxVtest=CzVtest=0; x0<b; x0+=dx) {
for(x=x0,Stest=CxStest=0; x<b; x+=dx) {
Stest += sqrt_tpl(r2-x*x)*2*dx;
CxStest += x*sqrt_tpl(r2-x*x)*2*dx;
}
Stesta = b*sqrt_tpl(r2-b*b)+r2*asin_tpl(b*rinv)-x0*sqrt_tpl(r2-x0*x0)-r2*asin_tpl(x0*rinv);
CxStesta = (-2.0f/3)*(cube(sqrt_tpl(r2-b*b))-cube(sqrt_tpl(r2-x0*x0)));
Vtest += Stest*dx*k;
CxVtest += CxStest*dx*k;
CzVtest += ((x0-a)*k+z0)*Stest*dx*k;
}*/
/*CTriMesh mesh;
vectorf pt3d[48],normals[92];
int idx[92*3];float x,y;
axes.x=m_cyl.axis.orthogonal().normalized(); axes.y=m_cyl.axis^axes.x;
for(i=0,x=r,y=0; i<24; i++) {
pt3d[i] = m_cyl.center+axes.x*x+axes.y*y-m_cyl.axis*m_cyl.hh;
pt3d[i+24] = pt3d[i]+m_cyl.axis*(2*m_cyl.hh);
k=x; x=x*cos_tpl(pi/12)-y*sin_tpl(pi/12); y=y*cos_tpl(pi/12)+k*sin_tpl(pi/12);
}
for(i=0;i<22;i++) { idx[i*3]=0; idx[i*3+1]=i+2; idx[i*3+2]=i+1; normals[i]=-m_cyl.axis; }
for(;i<44;i++) { idx[i*3]=24; idx[i*3+1]=i+3; idx[i*3+2]=i+4; normals[i]=m_cyl.axis; }
for(;i<92;i++) {
idx[i*3]=i-44>>1; idx[i*3+1]=idx[i*3]+1; idx[i*3+2]=idx[i*3]+24; normals[i]=(pt3d[idx[i*3+1]]-pt3d[idx[i*3]]^pt3d[idx[i*3+2]]-pt3d[idx[i*3]]).normalized(); i++;
idx[i*3]=(i-44>>1)+1; idx[i*3+1]=idx[i*3]+24; idx[i*3+2]=idx[i*3]+23; normals[i]=(pt3d[idx[i*3+1]]-pt3d[idx[i*3]]^pt3d[idx[i*3+2]]-pt3d[idx[i*3]]).normalized();
}
float V1; vectorf c1;
mesh.m_pVertices.data = pt3d;
mesh.m_pIndices = idx;
mesh.m_pNormals = normals;
mesh.m_nVertices=48; mesh.m_nTris=92;
V1 = mesh.CalculateBuoyancy(pplane,pgwd,c1);
if (fabs_tpl(V1-Vaccum)>V1*0.05f || (c1-massCenter).len2()>r2*sqr(0.05f))
i=0;
mesh.m_pNormals=0; mesh.m_pIndices=0; mesh.m_pVertices.data=0;*/
/*vectorf axisx,axisy,pt3d[16];
axisx = vectorf(1,0,0).rotated(rotax,n.z,-sina);
axisy = vectorf(0,1,0).rotated(rotax,n.z,-sina);
pt[0].set(r,0); itype[0]=1; pt3d[0] = center+axisx*pt[0].x+axisy*pt[0].y;
for(i=1;i<8;i++) {
itype[i]=1; pt[i].x=(pt[i-1].x-pt[i-1].y)*(sqrt2/2); pt[i].y=(pt[i-1].x+pt[i-1].y)*(sqrt2/2);
pt3d[i] = center+axisx*pt[i].x+axisy*pt[i].y;
} npt=8; pt[8]=pt[0];
vectorf Ptest(zero),Ltest(zero);
CalcMediumResistance(pt3d,8,n,*pplane,pgwd->v,pgwd->w,pgwd->centerOfMass,Ptest,Ltest);*/
/*Ptest += dP.rotated(rotax,n.z,-sina);
Ltest += dL.rotated(rotax,n.z,-sina);
if (Ptest.len()+Ltest.len()>0.001)
i=0;*/
float CCylinderGeom::CalculateBuoyancy(const plane *pplane, const geom_world_data *pgwd, vectorf &massCenter)
{
Vec3_tpl<vectorf> axes;
float r=m_cyl.r*pgwd->scale, h=m_cyl.hh*pgwd->scale*2, r2=r*r, rinv=1.0f/r,
denom,k,a,b,z0,z1,ya,yb,anglea,angleb,V[4],Vaccum;
vectorf n = pplane->n, center = pgwd->R*m_cyl.center+pgwd->offset, com[3],com_accum;
int i,nPieces=0;
axes.z = pgwd->R*m_cyl.axis; axes.z *= sgnnz(axes.z*n);
center -= axes.z*(h*0.5f); axes.y = n^axes.z;
massCenter = center+axes.z*(h*0.5f);
if (axes.y.len2()<0.0001f) {
// water plane is perpendicular to cylinder axis
z0 = max(0.0f,min(h,(pplane->origin-center)*pplane->n));
massCenter = center+axes.z*(z0*0.5f);
return pi*r2*z0;
}
axes.y.normalize(); axes.x = axes.y^axes.z;
denom = 1.0f/(axes.x*n);
a = ((pplane->origin-center)*n)*denom;
if (a>=r)
return 0;
else if (a<-r) {
if (sqr(axes.z*n)<0.0001f)
return pi*r2*h;
a = -r; z0 = ((pplane->origin-(center-axes.x*r))*n)/ (axes.z*n);
V[nPieces] = pi*r2*z0; com[nPieces++] = axes.z*(z0*0.5f*V[nPieces]);
} else z0 = 0;
b = ((pplane->origin-(center+axes.z*h))*n)*denom;
if (b<-r)
return pi*r2*h;
else if (b>r) {
b = r; z1 = ((pplane->origin-(center+axes.x*r))*n)/ (axes.z*n);
} else {
z1 = h; yb = sqrt_tpl(r2-b*b);
V[nPieces] = (r2*(pi*0.5f-asin_tpl(b*rinv))-b*yb)*(h-z0);
com[nPieces++] = axes.x*((2.0f/3)*cube(yb)*(h-z0)) + axes.z*((h+z0)*0.5f*V[nPieces]);
}
if (b>a+r*0.01f) {
denom = 1.0f/(b-a); k = (z1-z0)*denom;
ya = sqrt_tpl(r2-a*a); anglea = asin_tpl(a*rinv);
yb = sqrt_tpl(r2-b*b); angleb = asin_tpl(b*rinv);
V[nPieces] = k*((b*yb+r2*angleb)*(b-a)+(1.0f/3)*(cube(yb)-cube(ya))-r2*(b*angleb-a*anglea+yb-ya));
com[nPieces] = axes.z*(k*k*0.5f*((b*yb+r2*angleb)*(b*b-a*a)+0.5f*(b*cube(yb)-a*cube(ya))-r2*(0.75f*(b*yb-a*ya)-
0.25f*r2*(angleb-anglea)+b*b*angleb-a*a*anglea)) + V[nPieces]*(z0-a*k));
com[nPieces++] += axes.x*((2.0f/3)*k*(0.25f*(b*cube(yb)-a*cube(ya))+(3.0f/8)*r2*(b*yb-a*ya+r2*(angleb-anglea))-
cube(yb)*(b-a)));
}
for(i=0,Vaccum=0,com_accum.Set(0,0,0);i<nPieces;i++) {
Vaccum += V[i]; com_accum += com[i];//*V[i];
}
if (Vaccum>0)
massCenter = center+com_accum/Vaccum;
else Vaccum=0;
return Vaccum;
}
int crop_polyarc2d_with_line(const vector2df *pt,const int *itype,int npt, float r, vector2df *ptout,int *itypeout,
const vector2df &p0, const vector2df &l)
{
if (npt==0) return 0;
vector2df d0,c,ptt;
quotientf at[2],a[2],t[3];
float ka,kb,kc,kd;
int state,i,nout=0,i0,i1,j;
ka=l.len2(); kb=p0*l; kc=p0.len2()-r*r;
if ((kd=kb*kb-ka*kc)>=0) {
t[0].y=t[1].y=ka; kd=sqrt_tpl(kd);
t[0].x=-kb-kd; ptt = p0*t[0].y+l*t[0].x; at[0] = fake_atan2(ptt.y,ptt.x);
t[1].x=-kb+kd; ptt = p0*t[1].y+l*t[1].x; at[1] = fake_atan2(ptt.y,ptt.x);
}
state = isnonneg(l ^ pt[0]-p0);
for(i=0; i<npt; i++) {
i0=i1 = 0;
if (itype[i]) {
d0=pt[i+1]-pt[i]; c=pt[i]-p0;
t[2].y=d0^l; t[2].x=l^c;
if (inrange(t[2].x, 0.0f,t[2].y)) {
i0=2;i1=3; t[2].x=d0^c;
}
} else if (kd>=0) {
a[0] = fake_atan2(pt[i].y,pt[i].x);
a[1] = fake_atan2(pt[i+1].y,pt[i+1].x);
i0 = isneg(isneg(a[0]-at[0])+isneg(at[0]-a[1])+isneg(a[1]-a[0])-2);
i1 = isneg(1-isneg(a[0]-at[1])-isneg(at[1]-a[1])-isneg(a[1]-a[0]))+1;
}
if (state) {
ptout[nout]=pt[i]; itypeout[nout++]=itype[i];
}
for(j=i0; j<i1; j++,state^=1) {
ptout[nout]=p0+l*t[j].x/t[j].y;
itypeout[nout++] = itype[i]|state;
}
}
return nout;
}
void CCylinderGeom::CalculateMediumResistance(const plane *pplane, const geom_world_data *pgwd, vectorf &dPres,vectorf &dLres)
{
vectorf n,rotax,v,w,center,dir,org,pt0,dP(zero),dL(zero),ptside[4];
vector2df ptbuf0[16],ptbuf1[16],*pt=ptbuf0,*pt1=ptbuf1,*ptt;
float sina,r,rr,r2,hh,x0,y0,x1,y1,dx,dy,Fx,Fxy,Fxx,Fxxy,Fxyy,Fxxx,alpha,square;
int i,icap,npt,sgx,itypebuf0[16],itypebuf1[16],*itype=itypebuf0,*itype1=itypebuf1,*itypet;
dPres.zero(); dLres.zero();
r2 = sqr(r=m_cyl.r*pgwd->scale);
hh = m_cyl.hh*pgwd->scale;
n = pgwd->R*-m_cyl.axis;
rotax = n^vectorf(0,0,1);
sina = rotax.len();
if (sina>0.001f)
rotax/=sina;
else
rotax.Set(1,0,0);
center = pgwd->R*m_cyl.center*pgwd->scale+pgwd->offset+n*hh;
org = center + pplane->n*(pplane->n*(pplane->origin-center));
rr = 1/r;
x1 = 0.965925826f; y1 = 0.258819045f; // 15 degrees sin/cos
ptside[0] = vectorf(x0=r,y0=0,0).rotated(rotax,n.z,-sina)+center;
for(i=0; i<12; i++) {
ptside[1] = ptside[0];
dx = x0; x0 = (x0*sqrt3-y0)*0.5f; y0 = (y0*sqrt3+dx)*0.5f;
ptside[0] = vectorf(x0,y0,0).rotated(rotax,n.z,-sina)+center;
ptside[2] = ptside[1]-n*(hh*2); ptside[3] = ptside[0]-n*(hh*2);
CalcMediumResistance(ptside,4, vectorf(x1,y1,0).rotated(rotax,n.z,-sina),
*pplane,pgwd->v,pgwd->w,pgwd->centerOfMass,dPres,dLres);
dx = x1; x1 = (x1*sqrt3-y1)*0.5f; y1 = (y1*sqrt3+dx)*0.5f;
}
for(icap=-1; icap<=1; icap+=2,n.flip(),center+=n*(hh*2),rotax.flip()) {
v = (pgwd->v+(pgwd->w^center-pgwd->centerOfMass)).rotated(rotax,n.z,sina);
w = pgwd->w.rotated(rotax,n.z,sina);
pt[0].set(0,r); pt[1].set(0,-r); pt[2]=pt[0];
itype[0]=itype[1] = 0; npt = 2;
dir = pplane->n*(pplane->n*n)-n;
if (dir.len2() > 0.001f)
npt = crop_polyarc2d_with_line(pt,itype,npt, r, pt1,itype1,
vector2df((org+dir*(((center-org)*n)/(dir*n))-center).rotated(rotax,n.z,sina)),
-vector2df(cross_with_ort(pplane->n.rotated(rotax,n.z,sina),2)));
else {
ptt=pt;pt=pt1;pt1=ptt; itypet=itype;itype=itype1;itype1=itypet;
if (center*pplane->n > org*pplane->n)
npt = 0;
} pt1[npt] = pt1[0];
dir = pgwd->w^n;
if (dir.len2() > w.len2()*0.001f)
npt = crop_polyarc2d_with_line(pt1,itype1,npt, r, pt,itype,
vector2df((dir*((pgwd->v*n-center*dir)/dir.len2())).rotated(rotax,n.z,sina)), vector2df(w));
else {
ptt=pt;pt=pt1;pt1=ptt; itypet=itype;itype=itype1;itype1=itypet;
if (center*(pgwd->w^n) > pgwd->v*n)
npt = 0;
} pt[npt] = pt[0];
for(i=0,square=0,dP.zero(),dL.zero(); i<npt; i++) {
x0=pt[i].x; y0=pt[i].y; x1=pt[i+1].x; y1=pt[i+1].y;
if (itype[i]) {
dx = x1-x0; dy = y1-y0;
Fx = (x0*y1-x1*y0)*0.5f;
Fxy = dy*(x0*y0 + (dx*y0+dy*x0)*0.5f + dx*dy*(1.0f/3));
Fxx = dy*(x0*x0 + dx*x0 + dx*dx*(1.0f/3));
Fxxy = dy*(dx*dx*dy*0.25f + (dx*dx*y0+dx*dy*x0*2)*(1.0f/3) + (x0*y0*dx*2+x0*x0*dy)*0.5f + x0*x0*y0);
Fxyy = dy*(dy*dy*dx*0.25f + (dy*dy*x0+dy*dx*y0*2)*(1.0f/3) + (y0*x0*dy*2+y0*y0*dx)*0.5f + y0*y0*x0);
Fxxx = dy*(dx*dx*dx*0.25f + dx*dx*x0 + dx*x0*x0*1.5f + x0*x0*x0);
} else {
alpha = asin_tpl(max(-0.9999f,min(0.9999f,y1*rr)))-asin_tpl(max(-0.9999f,min(0.9999f,y0*rr)));
sgx = sgnnz(y1-y0);
Fx = r2*alpha*0.5f*sgx;
Fxy = (cube(x1)-cube(x0))*(-1.0f/3);
Fxx = r2*(y1-y0)-(cube(y1)-cube(y0))*(1.0f/3);
Fxxy = r2*(sqr(y1)-sqr(y0))*0.5f-(cube(y1)-cube(y0))*(1.0f/3);
Fxyy = (y1*cube(x1)-y0*cube(x0))*(-1.0f/4) + r2*(1.0f/8)*(x1*y1-x0*y0+r2*alpha*sgx);
Fxxx = 0.25f*(y1*cube(x1)-y0*cube(x0)+1.5f*r2*(y1*x1-y0*x0+r2*alpha*sgx));
}
square += Fx;
dP.z += w.x*Fxy-w.y*0.5f*Fxx;
dL.x += v.z*Fxy;
dL.y -= v.z*0.5f*Fxx;
dL.x += w.x*Fxyy-w.y*0.5f*Fxxy;
dL.y -= w.x*0.5*Fxxy-w.y*(1.0f/3)*Fxxx;
}
dP.z += v.z*square;
dPres -= dP.rotated(rotax,n.z,-sina);
dLres -= dL.rotated(rotax,n.z,-sina);
}
}
int CCylinderGeom::DrawToOcclusionCubemap(const geom_world_data *pgwd, int iStartPrim,int nPrims, int iPass, int *pGrid[6],int nRes,
float rmin,float rmax,float zscale)
{
CBoxGeom geombox;
geombox.m_box = m_Tree.m_Box;
return geombox.DrawToOcclusionCubemap(pgwd,iStartPrim,nPrims,iPass,pGrid,nRes,rmin,rmax,zscale);
}
void CCylinderGeom::DrawWireframe(void (*DrawLineFunc)(float*,float*), geom_world_data *gwd, int iLevel)
{
if (iLevel==0) {
int i; float step,ang;
vectorf axes[3],center,pt0,pt1;
step = (pi*2/m_nTesselation);
axes[2] = gwd->R*m_cyl.axis;
// axes[0] = axes[2].orthogonal().normalized();
axes[0] = GetOrthogonal(axes[2]).normalized();
axes[1] = axes[2]^axes[0];
axes[0] *= m_cyl.r*gwd->scale; axes[1] *= m_cyl.r*gwd->scale;
axes[2] *= m_cyl.hh*gwd->scale;
center = gwd->R*m_cyl.center*gwd->scale + gwd->offset;
pt0 = center+axes[0];
for(i=0,ang=step;i<m_nTesselation;i++,ang+=step,pt0=pt1) {
pt1 = center + axes[0]*cos_tpl(ang) + axes[1]*sin_tpl(ang);
DrawLineFunc(pt0-axes[2], pt1-axes[2]);
DrawLineFunc(pt0+axes[2], pt1+axes[2]);
DrawLineFunc(pt1-axes[2], pt1+axes[2]);
}
} else {
BV *pbbox;
ResetGlobalPrimsBuffers();
m_Tree.GetNodeBV(gwd->R,gwd->offset,gwd->scale, pbbox);
DrawBBox(DrawLineFunc,gwd, &m_Tree,(BBox*)pbbox,iLevel-1);
}
}
void CCylinderGeom::GetMemoryStatistics(ICrySizer *pSizer)
{
pSizer->AddObject(this, sizeof(CCylinderGeom));
}
void CCylinderGeom::Save(CMemStream &stm)
{
stm.Write(m_cyl);
stm.Write(m_nTesselation);
m_Tree.Save(stm);
}
void CCylinderGeom::Load(CMemStream &stm)
{
stm.Read(m_cyl);
stm.Read(m_nTesselation);
m_Tree.Load(stm,this);
}
Reference in New Issue View Git Blame Copy Permalink